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吉林 省 长 春 市 人 ， 吉林 大 学 机 械 制 
造 及 其 自动 化 专业 工学 博士 ， 长 春 工程 
学 院 工 程 训 练 中 心 副 主任 /副教授 ， 从 
事 智能 精密 制造 研究 。 在 自主 研 抛 机 器 
人 技术 方面 主持 吉林 省 科技 发 展 计划 重 
点 科技 攻关 项 目 、 吉 林 省 科技 厅 自 然 科 
学 基金 项 目 、 吉 林 省 教育 厅 项 目 。 近 年 
来 ， 在 自主 研 抛 机 器 人 研究 领域 发 表 论 
文 5 篇 ， 其 中 2 篇 论文 被 E[ 检 索 。 获 得 长 
春 市 科学 技术 进步 一 等 奖 一 项 ， 吉 林 省 
科学 技术 进步 三 等 奖 一 项 。 





Н НЕЙ A DEAN 


The New Robot Technology for Autonomous Grinding 


Ix% Ж 


е 


机 械 工 业 出 版 社 








本 书 主要 介绍 自主 研 抛 机 器 人 技术 的 相关 理论 基础 与 前 治 技术 ， 是 作 








者 近年 来 在 国家 “863” 等 项 目 中 所 取得 的 学 术 研 究 和 技术 实践 成 只 
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书 中 基于 实现 用 小 型 装备 对 大 型 自由 曲面 进行 精 整 加 工 的 目标 ， 利 用 
移动 操作 机 器 人 具有 较 好 的 操作 灵活 性 和 工作 空间 大 的 特点 ， 提 出 了 一 种 


















































研磨 大 型 自由 曲面 自主 作业 机 器 人 





究 。 重 点 针对 研磨 大 型 自由 曲面 


























的 机 械 结 构 ， 并 进行 了 关键 技术 的 研 
自主 作业 机 器 人 的 曲面 重 构 与 定位 方法 、 


5 - TTRRT 机 器 人 运动 学 动力 学 建 模 、 运 动 规划 与 运动 控制 、 主 被 动 结合 














的 柔顺 控制 方式 等 进行 了 深入 论述 。 力 求 设计 到 














发 展 要 求 ， 设 计 内 容 与 国内 外 最 新 研究 成 果 同 步 。 
本 书 可 供 机 器 人 研究 及 自动 化 方向 
也 可 作为 机 械 设计 及 其 自动 化 、 机 械 设 















































机 械 电子 工程 等 相关 专业 研究 生 的 参考 用 书 。 
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工业 机 器 人 技术 应 用 于 曲面 的 精 整 加 工 领域 始 于 20 世纪 70 年 
代 ， 目 前 ， 技 术 较 为 成 熟 的 是 使 用 固定 型 串联 关节 型 机 器 人 ， 相 关 
的 研究 成 果 已 得 到 应 用 。 例 如 ， 串 联 关节 型 机 器 人 技术 已 经 应 用 到 
自由 曲面 研磨 或 抛光 加 工 的 自动 化 制造 领域 。 但 是 随 着 加 工 对 象 尺 
度 的 扩大 ， 上 述 加 工 方法 在 自动 研 抛 加 工 中 会 受到 研 执 设 备 加 工 范 
围 的 限制 ， 同 时 还 存在 着 加 工 范围 与 精度 、 刚 度 ， 和 运动 快速 性 和 稳 
定性 等 诸多 技术 相关 联 的 难题 。 由 于 大 型 自由 曲面 精 整 加 工 技术 发 
展 落 后 于 前 期 的 曲面 形状 加 工 技术 ， 因 而 将 微小 移动 作业 机 器 人 技 
术 应 用 于 大 型 自由 曲面 精 整 加 工 ， 以 弥补 固定 上 串联 关节 型 机 器 人 的 
不 足 。 作 为 提高 大 型 曲面 制造 的 智能 化 和 自动 化 手段 ， 其 在 降低 成 
本 、 提 高 效率 方面 的 优势 受到 了 产业 界 和 学 术 界 的 高 度 重 视 。《 中 国 
制造 2025》 的 提出 ， 对 智能 制造 提出 了 新 的 要 求 ， 要 求 机 器 人 能 够 
实现 智能 自主 作业 。 本 书 作 者 王 文 忠 依 托 国家 高 技术 研究 发 展 计划 
(“863” 计 划 ) 资助 项 目 : “大 型 曲面 自主 研 抛 作业 微小 机 器 人 技 
术 ” 和 国家 自然 科学 基金 项 目 : “ 自 定位 微小 研 抛 机 器 人 精 整 加 工 
大 型 曲面 研究 ”， 针 对 制造 领域 中 的 这 一 重要 问题 展开 深入 而 细致 的 
研究 。 作 为 他 的 博士 研究 生 指 导 教 师 之 一 ， 我 很 欣慰 地 看 到 了 我 的 
学 生 的 专著 能 够 出 版 ， 他 的 研究 成 果 和 观点 能 得 以 传播 。 

该 书 以 采用 小 型 装备 对 大 型 自由 曲面 进行 精 整 加 工 为 目的 ， 利 
用 移动 作业 机 器 人 的 技术 特点 ， 研 发 了 一 种 可 研磨 大 型 自由 曲面 的 
自主 移动 作业 机 器 人 系统 。 该 系统 通过 连接 腿 部 的 直角 坐标 平 合 ， 
可 进行 蕊 向 与 卫 向 运动 ， 应 用 视觉 技术 对 待 加 工 曲 面 进行 反 求 ， 以 
实现 自主 规划 机 器 人 的 路 径 。 书 中 对 该 系统 在 曲面 模型 重 构 与 定位 、 
运动 学 与 动力 学 分 析 、 运 动 规划 与 运动 控制 及 柔顺 控制 等 方面 进行 
了 深入 的 研究 ， 最 后 通过 机 器 人 研磨 加 工 试验 对 相关 的 原理 与 技术 
进行 验证 。 本 书 结构 清晰 ,逻辑 严谨 ， 观 点 明确 ， 行 文 流畅 ， 研 究 
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IV 自主 研 抛 机 器 人 技术 





思路 与 方法 具有 一 定 的 创新 性 。 
该 书 对 于 从 事 机 器 人 加 工 应 用 的 教学 、 研 究 人 员 及 企业 工程 技 
术 人 员 有 启示 作用 ， 也 可 作为 研究 生 的 参考 书 ， 
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本 书 是 在 博士 论文 基础 上 ， 经 过 修改 、 充 实 完 
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装配 都 很 困难 ， 加 
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号 : 2006AA04Z214) “大 型 曲面 自主 研 抛 作业 微小 机 器 人 技术 ”。 根 
据 作 者 近年 来 所 取得 的 学 术 研 究 和 技术 实践 成 果 整 理 、 撰 写 而 成 。 
首先 特别 感谢 我 的 导师 王立 易 院 士 和 赵 继 教授 ， 还 要 感谢 张震 教授 、 
李 大 奇 博 士 、 杨 旭 博 士 、 徐 刚 硕 士 、 李 觅 硕士 等 同学 对 本 书 相 关 研 
究 所 做 出 的 贡献 。 

编写 本 书 ， 一 方面 希望 推动 对 自主 研 抛 机 器 人 的 进一步 研究 ， 
同时 也 希望 能 够 为 同行 提供 借鉴 和 参考 。 

本 书 的 出 版 得 到 吉林 省 科技 发 展 计划 重点 科技 攻关 项 目 (ME 
编号 : 20140204011GX)“ 基 于 自主 作业 机 器 人 的 自由 曲面 研 抛 加 工 
研究 ”和 长 春 工程 学 院 学 术 专 著 出 版 基金 的 资助 ， 特 致 以 衷心 的 
感谢 ! 

由 于 作者 水 平 有 限 ， 书 中 必然 存在 许多 错误 不 当 之 处 ， 在 文字 
和 结构 上 亦 存 缺 屋 ， 恳 请 读者 批评 指正 ! 
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Sis # R 


“机 器 人 是 一 种 自动 的 、 位 置 可 控 的 、 具 有 自动 编程 能 力 的 高 度 灵 活 的 自 
动 化 机 器 ， 这 种 机 器 具备 一 些 与 人 或 生物 相似 的 能 力 ， 如 感知 能 力 、 规 划 能 
力 、 动 作 能 力 和 协同 能 力 ”。 到 目前 为 止 ， 国 内 外 已 对 机 器 人 基础 理论 与 基础 
元 器 件 进行 了 全 面 的 研究 。 已 相继 研制 出 门类 齐全 的 工业 机 器 人 及 水 下 作业 、 
军用 和 特种 机 器 人 ， 在 不 同 的 行业 取得 了 广泛 的 应 用 。 

目前 ， 将 机 器 人 技术 应 用 于 大 型 模具 自由 曲面 精 整 加 工 ， 提 高 大 型 曲面 制 
造 的 智能 化 和 自动 化 ， 以 降低 成 本 、 提 高 效率 ， 受 到 了 产业 界 和 学 术 界 的 高 度 
重视 ， 并 且 一 致 认为 “模具 是 工业 生产 的 基础 工艺 装备 [1] 。 


1.1 应 用 机 器 人 技术 精 整 加 工大 型 自由 曲面 的 背景 








进入 21 世纪 以 来 ， 对 于 大 型 自由 曲面 零件 的 加 工 要 求 越 来 越 高 ， 如 曲面 
模具 、 航 空 镜头 及 航空 发 动机 叶片 等 。 其 中 模具 成 形 具 有 优质 、 高 效 、 低 成 本 
的 特点 ， 世 界 模具 市 场 需求 潜力 巨大 ， 因 此 模具 制造 已 成 为 各 工业 发 达 国 家 制 
造 业 中 举足轻重 的 行业 。 在 机 械 、 电 子 、 轻 工 、 汽 车 、 纺 织 、 航 空 航天 等 行业 
得 到 了 广泛 的 应 用 ， 并 承担 了 这 些 工业 领域 中 60% ~90% 产品 零件 、 组 件 和 
部 件 的 加 工 生产 。 在 模具 的 整个 制造 过 程 中 ， 各 工序 工作 量 所 占 比 例 依 次 为 : 
设计 约 11% 、 制 造 约 52% 、 型 面 的 精 整 加 工 约 占 37% 1 。 模 具 型 面 的 精 整 加 
工 工作 量 大 ， 而 且 是 决定 模具 和 制 件 质量 的 重要 因素 ， 对 产品 和 模具 本 身 寿 命 
影响 极 大 。 据 统计 ， 模 具 型 腔 表 面 粗 烽 度 改善 一 级 ,模具 寿命 可 提高 50% 7, 
因此 ， 加 速 研发 高 精度 大 型 模具 自由 曲面 精 整 加 工 技术 是 必然 的 趋势 。 

目前 ， 通 过 前 期 的 数控 加 工 方法 可 以 实现 大 型 模具 自由 曲面 的 自动 化 形状 
加 工 ， 但 是 为 了 获得 预期 的 表面 质量 ， 加 工 后 的 表面 都 需 再 经 过 平滑 加 工 
(Smoothing) 、 研 磨 (Lapping, Grinding) 或 抛光 (Polishing) 等 光 整 加 工 工 
序 。 当 前 大 型 自由 曲面 精 整 加 工 技术 的 发 展 落后 于 曲面 形状 加 工 技术 ， 由 于 大 
型 自由 曲面 精 加 工 的 工艺 环节 自动 化 程度 相对 较 低 ， 常 常 还 需 依靠 耗费 大 量 工 
时 的 手工 操作 方式 来 完成 表面 研磨 (Lapping, Grinding) 或 抛光 (Polishing) 。 
在 美国 、 日 本 和 德国 等 发 达 工 业 国 家 ， 总 工时 的 37% ~ 42% 被 用 于 精密 模具 
曲面 的 手工 研磨 或 抛光 加 工 ， 在 我 国 达到 了 50% 以 上 。 手 工人 研磨 或 抛光 加 工 
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过 程 中 ， 形 状 精度 与 加 工 质 量 的 一 致 性 差 ， 且 效率 低下 ， 与 降低 制造 成 本 、 缩 
短 生 产 周期 、 提 高 质量 等 要 求 的 矛盾 越 来 越 突 出 ， 模 具 制 造 过 程 中 的 薄弱 环节 
与 发 展 瓶颈 主要 集中 在 对 大 型 自由 曲面 的 精 加 工 阶 段 。 目 前 ， 进口 模 具 约 占 国 
内 市 场 总 量 的 20% 左右 ，40% 以 上 中 高 档 模具 依靠 进口 。 因 此 研发 具有 自主 
知识 产权 的 大 型 自由 曲面 精 加 工 技 术 与 装备 的 需求 越发 迫切 ， 特 别 是 针对 大 
型 、 精 密 、 复 杂 、 长 寿命 的 模具 生产 需求 。 

目前 若干 利用 机 器 人 技术 的 研究 成 果 已 应 用 到 模具 自由 曲面 研磨 或 抛光 加 
工 的 自动 化 制造 领域 。 出 现 了 各 种 新 颖 的 研磨 或 抛光 方法 、 开 发 了 各 种 实验 装 
置 ， 进 行 了 机 器 人 研磨 运动 规划 、 磨 前 干涉 检验 、 人 研磨 工艺 与 控制 策略 及 控制 
算法 的 研究 等 以 -5] 。 

为 了 实现 大 型 自由 曲面 的 研磨 或 抛光 加 工 自动 化 ， 在 对 不 同 加 工 对 象 的 待 
加 工区 域 实现 加 工时 ， 应 用 传统 的 数控 机 床 或 一 般 工 业 机 器 人 技术 ， 必 须 依靠 
大 型 研磨 或 抛光 加 工 设备 。 由 于 机 械 设备 结构 增 大 到 一 定 程 度 后 ， 其 精度 、 刚 
度 、 响 应 速度 、 稳 定性 和 动力 等 方面 的 问题 难以 解决 ， 因 而 大 型 自由 曲面 研磨 
或 抛光 系统 的 研发 进展 缓慢 。 

另外 ， 如 何 使 固定 加 工 范围 的 加 工 系统 适应 竺 加工 工 件 尺 十 的 变化 ， 即 如 
何 提高 加 工 系统 的 柔性 ， 也 是 一 种 需要 解决 的 难题 。 


1.2 机 器 人 精 整 加 工 自 由 曲面 技术 研究 进展 


B 20 世纪 70 年 代 始 ， 工 业 机 器 人 技术 就 开始 应 用 于 曲面 的 精 整 加 工 ， 许 
多 相关 实用 的 研究 成 果 已 得 到 应 用 。 其 中 技术 较为 成 熟 的 关节 型 机 右 人 是 应 用 
较 多 的 一 种 ， 研 磨 工 具 被 夹 持 在 关节 型 机 器 人 终端 的 结构 方案 ， 是 机 器 人 研磨 
加 工 系统 的 关键 。 参 考 文献 [7-9] 给 出 了 日 本 几 例 有 关机 器 人 研磨 技术 的 
研究 成 果 ， 为 了 实现 对 人 研磨 力 的 反馈 控制 ， 力 传 感 品 被 安装 在 关节 型 机 器 人 的 
终端 研磨 执 行 器 上 。 一 个 典型 的 以 关节 型 机 器 人 应 用 于 人 研磨 模具 曲面 的 实例 是 
日 本 东京 科学 大 学 (Tokyo University of Science ) 研发 的 机 器 人 研磨 系统 [10] ， 
图 1-1 是 参考 文献 [12] 提 到 的 加 工 系统 机 械 结 构 ， 机 器 人 研磨 系统 采用 在 
MOTOMAN 工业 机 器 人 上 加 装 特 制 的 终端 研磨 执行 机 构 。 夹 持 终端 球形 研磨 工 
具 的 细节 如 图 1-1b 所 示 ， 三 维 力 传 感 带 安 装 在 工具 后 端的 夹 持 装 置 上 ， 可 实 
现 对 加 工作 用 力 和 运动 方向 上 摩擦 力 的 反馈 。 可 由 CAD 模型 提供 的 几何 信息 
得 到 待 加 工 部 位 信息 ， 反 馈 的 受 力 情况 能 够 被 综合 考虑 ， 加 工 接触 点 法 向 的 恒 
力 控 制 可 通过 对 加 工 工 具 的 力 控制 得 到 保证 。 

图 1-2 给 出 的 关节 型 机 器 人 应 用 实例 ， 是 西班牙 马德里 技术 大 学 ( Poly- 





















































a) 加 上 系统 图 


Material : Aluminum А 
b) 夹 持 终 端 款 形 研 磨 工 具 
图 1-1 日 本 东京 科学 大 学 研制 的 机 器 人 研磨 系统 


























图 1-2 马德里 技术 大 学 研制 的 机 器 人 研磨 系统 








technic University of Madrid, Spain) 研发 的 机 器 人 研磨 系统 [04 ， 在 自动 规划 
研磨 工具 的 轨迹 与 控制 策略 方面 ， 综 合 利 用 的 几何 信息 ， 由 被 加 工 曲面 的 
CAD 数据 模型 与 研磨 工具 的 运动 规划 所 提供 ， 实 现 了 研磨 接触 力 恒定 的 要 求 。 

目前 ， 在 应 用 不 同 种 类 的 机 器 人 实现 对 不 同 加 工 对 象 和 材料 的 加 工 方面 ， 
国内 许多 院 校 与 研究 所 进行 了 机 器 人 人 研磨 机 构 、 控 制 系统 和 工艺 机 理 等 方面 的 
研究 ， 取 得 的 部 分 技术 成 果 已 经 进行 产业 化 转化 。 在 利用 超声 振动 进行 研磨 方 
面 ， 华 中 科技 大 学 基于 日 本 的 RV -2MI 型 机 器 人 进行 了 相关 研究 !2] ， 吉 林 
大 学 提出 了 倾斜 超声 波 研 磨 方法 ， 利 用 关节 机 器 人 进行 了 研磨 金属 模具 的 自由 
曲面 技术 研究 5 ;中 国 科学 院 沈阳 自 动 化 所 在 机 器 人 研磨 有 机 玻璃 的 工艺 方 
面 ， 以 SIASUN206B 机 器 人 为 实验 平台 进行 了 研究 !4] 。 参 考 文献 [15] 基于 
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五 轴 框 架 式 加 工 机 器 人 ， 对 研磨 的 工艺 参数 和 路 径 规划 的 优化 进行 了 研究 ; 在 
柔性 研 抛 系统 研究 方面 ， 参 考 文献 [16] 给 出 的 机 器 人 柔性 抛光 系统 由 一 台 
抛光 机 和 一 台 ABB 公司 的 IRB 4400 机 器 人 组 成 ， 对 于 抛光 过 程 中 保持 恒定 压 
力 的 算法 和 控制 方法 进行 了 研究 。 在 一 些 特 殊 复 杂 曲 面 的 研磨 加 工 方面 ， 参 考 
文献 [17] 针对 飞机 发 动机 、 汽 轮机 叶片 ， 研 发 了 由 六 自由 度 ABB 关节 式 串 
联机 器 人 和 砂 带 研磨 机 组 成 的 机 器 人 研磨 系统 ， 实 现 了 复杂 曲面 的 研磨 加 工 。 
此 外 ， 在 机 器 人 曲面 自动 抛光 系统 方面 ， 对 于 面向 熔 射 快速 制 模 的 研究 ， 参 考 
文献 [18] 以 一 个 六 自由 度 的 关节 式 MOTOMAN UP - 20 型 机 器 人 为 平台 进行 
了 相关 的 研究 ， 生 成 了 机 器 人 抛光 刀具 轨迹 ， 研 究 了 软 质 抛光 工具 的 选择 。 

在 突破 传统 加 工 观 念 方面 ， 日 本 的 一 些 学 者 设计 出 可 以 自由 移动 的 微型 机 
咒 人 ， 提 出 了 利用 微机 器 人 进行 超 精密 加 工 的 概念 ， 通 过 机 器 人 群 在 工件 上 的 
行 ， 可 实现 纳米 级 超 精 密 加 工 ' 1， 图 1-3 是 几 种 微机 器 人 用 于 精密 加 工 的 
实例 [20 -23] 5 
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图 1-4 是 





林 大 学 对 大 型 复杂 曲面 自 定 位 微小 研磨 移动 机 器 人 的 研究 成 
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果 ， 目 前 ， 已 开发 了 轮 式 微小 移动 研磨 机 器 人 [24 -2 系统 。 对 基于 轮 式 移动 平 
台 机 器 人 的 研磨 工具 与 加 工 机 理 、 运 动 学 和 动力 学 模型 及 相应 的 控制 方法 进行 
了 相关 人 研究。 
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图 1-4 大 型 复杂 曲面 自 定位 自主 作业 研磨 机 器 人 


机 器 人 构成 的 研磨 加 工 系统 与 数控 机 床 相 比 ， 具 有 较 好 的 加 工 和 柔性 与 较 高 
的 复杂 曲面 适应 性 。 此 外 ， 该 类 系统 在 加 工 过 程 中 ， 重 点 强调 的 是 柔顺 控制 ， 
较 低 的 机 械 位 置 执 行 精度 要 求 ， 较 小 的 精 整 加 工作 用 力 要 求 ， 适 合 机 器 人 刚性 
弱 、 运 动 精度 低 的 特点 ， 因 此 在 精 整 加 工 方 面具 有 很 好 的 应 用 前 景 '*7] 。 

机 器 人 的 机 械 机构 、 研 磨 执 行 机 构 、 控 制 系统 和 气动 、 液 压 系统 构成 了 目 
前 研发 的 机 器 人 研磨 加 工 系统 的 主体 ， 此 外 ， 还 包括 控制 研磨 作用 力 的 多 维 力 
传 感 技 术 、 保 证 位 置 精 度 的 位 置 反 饶 技术 、 导 航 定位 的 视觉 传 感 咒 技术 、 
CADZCAM、 运 动 规划 和 曲面 几何 模型 重 构 技 术 。 针 对 采用 机 器 人 实施 研磨 加 
工 的 研究 主要 从 以 下 几 个 方面 展开 : 

1) 在 研磨 工艺 和 加 工 机 理 方面 ， 除 常规 的 机 械 加 工 方法 外 ， 还 涉及 超声 、 
机 械 电解 复合 加 工 等 特种 加 工 方法 。 

2) 针对 柔顺 性 能 要 求 ， 进 行 研磨 终端 执行 装置 和 抛 磨 工 具 开 发 ， 如 利用 
磁力 、 气 压 、 弹 簧 等 方法 ， 满 足 研磨 加 工 中 对 接触 力 的 柔顺 控制 要 求 ![3'2 1 。 

3) 根据 已 知 的 被 加 工 工件 的 CAD 模型 或 对 于 模型 未 知 的 需要 通过 反 求 重 
构 得 到 的 工件 几何 模型 ， 根 据 不 同 的 研磨 执行 装置 和 机 器 人 结构 及 运动 的 特 
点 ， 进 行 加 工区 域 分 划 并 规划 出 最 优 的 工作 路 径 ， 从 而 保证 满足 加 工 质量 与 加 
工效 率 的 目标 :30 -321 。 

4) 由 于 机 器 人 具有 耦合 性 强 、 高 度 非 线性 的 特点 ， 结 合 人 工 智能 的 进展 ， 
如 何 实施 力 /位 控制 研究 ， 使 机 器 人 执行 研磨 轨迹 运动 时 ， 保 持 恒 定 研磨 力 ， 
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实现 力 / 位 混合 控制 ， 还 需要 进一步 研究 。 


1.3 移动 作业 机 器 人 技术 国内 外 研究 现状 及 关键 技术 研究 


1.3.1 移动 作业 机 器 人 国内 外 研究 现状 


Љ 20 世纪 60 年 代 开 始 ， 国 外 就 开展 了 关于 移动 机 器 人 的 研究 ， 移 动机 器 
人 涉及 多 个 学 科 门 类 的 知识 。 

按照 移动 方式 分 类 ， 可 以 分 为 轮 式 、 履 带 式 、 腿 式 ( 单 腿 、 双 腿 及 多 腿 
式 ) 和 水 下 推进 式 等 多 种 结构 ， 其 中 轮 式 结构 具有 易 控 制 、 对 稳定 性 问题 影 
响 小 、 单 位 移动 距离 消耗 能 量 小 的 特 en 
性 ， 且 可 以 比 其 他 形式 的 结构 移动 得 更 e 
快 。 此 外 ， 轮 式 结构 的 机 器 人 的 重心 一 саол тс 
般 在 车 轮 连 线 在 地 面 投影 形成 的 多 边 形 | NES > 1 
内 ， 通 过 适当 的 车 轮 配 置 可 以 保持 机 带 | 
人 稳定 而 灵活 地 旋转 、 平 移 ， 因 此 轮 式 
结构 在 移动 机 器 人 中 得 到 了 广泛 的 
WJ). 

图 1-5 所 示 3 Rover 轮 全 方位 移动 
操作 机 器 人 (Stanford Robot) [34] ， 由 斯 š 
坦 福 大 学 的 Oussama K 研制 。 1-5 3 Rover 轮 全 方位 移动 机 器 人 

日 本 东部 大 学 的 Yasuhisa Н 和 
WangZD 研制 的 具有 4 个 Mecanum 轮 的 全 方位 移动 操作 机 器 人 15] ， 如 图 1-6 
所 示 。 

FAI, FMEA SEP PEAY MRE Lat Л (Wall - Climbing Robot, WCR), 
WCR 的 运动 机 构 主要 有 是 式 、 框 架 式 、 履 带 式 及 轮 式 等 。 

图 1-7 所 示 电 磁 吸 附 足 式 WCR， 由 Guo Lin 等 人 设计 131。 行走 机 构 的 两 
只 脚 带 有 三 个 电磁 吸盘 并 成 交叉 三 角形 结构 ， 可 在 壁面 实现 交替 抬 起 、 平 移 和 

Chen 1. М. 等 人 [037 设计 了 图 1-8 所 示 四 只 脚 结构 的 Planar Walker， 机 器 人 
的 直线 行走 和 转弯 功能 通过 每 两 只 脚 之 间 的 气缸 伸缩 运动 带动 实现 ， 机 器 人 的 
固定 由 每 只 脚 上 的 吸盘 保证 。 






































图 1-6 4 Mecanum 轮 全 方位 移动 操作 机 器 人 











图 1-7 Guo Lin 设计 的 WCR 1-8 Planar Walker 


国内 在 研制 移动 机 器 人 方面 也 取得 了 许多 成 果 ， 例 如 : 图 1-9 中 所 示 的 具 
有 上 自主 知识 产权 的 “ 灵 蜥 一 B” 型 排 爆 机 器 人 ， 是 由 中 国 科学 院 沈 阳 自 动 化 研 
究 所 自行 研制 的 [33] 。 
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国 “ 灵 蜥 一 8B” 型 排 爆 机 器 人 
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哈尔滨 工业 大 学 在 全 方位 移动 机 器 人 方面 进行 了 大 量 的 研究 ， 在 参考 文献 
[39] 中 给 出 了 一 种 自行 研制 的 移动 式 作 业 型 智能 服务 机 带 人 ， 在 该 机 噩 人 研 
究 项 目 中 主要 研究 了 全 方位 移动 机 构 、 七 自由 度 机 器 人 作业 手臂 及 多 传感器 信 
息 融合 的 机 器 人 路 径 规划 等 问题 。 在 此 基础 上 ， 研 制 出 采用 Mecanum 轮 的 全 
方位 移动 服务 机 器 人 。 

图 1- 10 所 示 的 具有 抗 倾覆 机 构 和 纠偏 机 构 的 永 磁 吸 附 双 履 带 式 WCRi , 
能 够 用 于 锅炉 水 冷 辟 清扫、 检测 ， 通 过 每 条 履带 上 安装 的 永 磁铁 来 实现 在 壁面 
上 的 吸附 。 

图 1-11 所 示 的 永 磁 吸 附 式 WCRL4%1 ， 依 靠 稀土 永 磁 吸 盘 作 为 壁面 吸附 机 
FJ SC BIG üt E PRU D 





图 1-10 锅炉 水 冷 壁 清扫 WCR 图 1-11 上 海 交 大 永 磁 吸附 式 WCR 














图 1-12 与 图 1-13 是 两 种 由 北航 研制 的 玻璃 幕墙 清洗 机 器 人 [wi ， 其 中 
Cleanbot - 工 型 由 本 体 、 保 险 与 随 动 系统 以 及 地 面 文 援 系统 组 成 。 由 纵 、 横 呈 
十 字 交 叉 的 两 组 气缸 组 成 的 本 体 ， 可 沿 纵横 方向 自由 运动 和 越 障 。 








到 1-12 Cleanbot -I 图 1-13 清洗 拱 形 外 墙 机 器 人 














1.3.2 移动 作业 机 器 人 关键 技术 研究 


由 移动 平台 和 安装 在 移动 平台 上 的 机 械 臂 构成 的 移动 作业 机 器 人 ， 与 传统 
的 移动 机 器 人 或 者 机 械 手 相 比 ， 兼 顾 了 移动 性 和 操作 性 。 一 方面 ， 移 动 平台 和 
机 械 手 的 结合 ， 在 扩大 机 右 人 工作 空间 的 同时 ， 也 带 来 高 度 的 运动 学 匈 余 。 男 
一 方面 ， 移 动 平台 和 机 械 手 动力 学 特性 的 不 同 ， 存 在 着 相对 复杂 的 机 械 结 构 以 
及 彼此 之 间 的 耦合 作用 问题 。 移 动机 械 手 系统 的 难点 和 热点 主要 体现 在 如 下 几 
个 方面 : 运动 学 和 动力 学 建 模 与 分 析 、 运 动 规划 与 协调 运动 控制 、 力 控制 及 导 
航 与 定位 等 14!]。 

1. 移动 机 械 手 系统 的 运动 学 和 动力 学 建 模 

国外 很 多 学 者 就 全 方位 移动 机 械 手 的 建 模 与 控制 做 了 大 量 的 工作 ， 对 于 轮 
式 移动 机 器 人 的 运动 学 与 动力 学 模型 ，CAMPION 442-44) 进行 了 分 析 和 讨论 ， 
并 对 控制 方法 做 了 初步 研究 ;对 于 全 方位 移动 平台 与 机 械 臂 组 成 的 移动 机 械 
F, TAN 等 司 ] 建 立 了 笛 卡 儿 空间 的 一 体 化 动力 学 模型 ， 在 线性 化 和 解 耘 方面 
采用 了 非 线性 负 反馈 的 方法 ， 进 一 步 实 现 了 力 与 位 置 的 同时 控制 ; 在 移动 机 械 
手 的 动力 学 模型 与 鲁 棒 的 控制 方法 方面 ，HOLMBERG 146-48] 进行 了 深入 的 
WEE. 对 于 构成 移动 机 械 手 的 两 个 子 系统 ， 移 动 平台 和 机 械 辟 的 独立 建 模 及 相 
应 的 鲁 棒 控 制 器 研究 方面 ，LIU 等 名] 把 系统 的 动力 学 耦合 和 不 确定 性 看 作 是 
外 扰 ， 实 现 了 在 外 扰 有 界 情况 下 系统 的 渐 近 稳定 。 目 前 ， 针 对 全 方位 移动 机 械 
手 模 型 的 控制 ， 主 要 有 基于 运动 学 模型 的 控制 和 基于 动力 学 模型 的 控制 。 在 实 
现 控制 策略 方面 ， 一 种 是 把 移动 平台 和 机 械 辟 看 成 是 两 个 子 系统 ， 分 别 建 模 与 
控制 的 分 散 控制 法 。 该 方法 虽然 简单 ， 但 是 没有 充分 发 挥 移动 平台 和 机 械 手 同 
时 协调 运动 的 优点 ; 另 一 种 是 整个 移动 机 械 手 看 成 是 一 个 整体 ， 集 中 建 模 与 控 
制 。 该 方法 建 模 和 控制 的 计算 复杂 性 较 大 ， 实 现 困难 14!]。 

2. 移动 作业 机 器 人 的 运动 控制 

由 移动 平台 与 作业 臂 构成 的 移动 作业 机 器 人 是 一 种 非 线性 不 确定 系统 ， 该 
类 系统 具有 复杂 的 多 输入 多 输出 、 时 变 、 强 耦合 特点 。 在 动力 学 建 模 中 ， 存 在 
参数 的 不 确定 性 、 外 部 干扰 和 未 建 模 动态 的 影响 。 移 动作 业 机 器 人 的 运动 规 
划 、 移 动作 业 机 右 人 的 协调 控制 与 轨迹 跟踪 问题 是 运动 控制 的 主要 研究 领域 。 
由 于 运动 控制 是 机 絮 人 工作 的 基础 ,需要 从 理论 和 实际 应 用 方面 进行 研究 ， 因 




















(1) 移动 作业 机 顺 人 的 运动 规划 问题 ”在 运动 规划 问题 上 ， 移 动作 业 机 顺 
人 不 同 于 移动 机 器 人 。 移 动作 业 机 顺 人 的 运动 规划 分 为 移动 平台 运动 规划 和 机 
械 臂 运动 规划 两 部 分 。 移 动作 业 机 器 人 的 运动 元 余 性 来 源 于 移动 平台 和 机 械 臂 
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的 组 合 ， 根 据 任务 的 需要 ， 在 完成 同一 个 任务 时 ， 存 在 单独 运动 移动 平台 或 机 
械 辟 及 同时 协调 移动 平台 和 机 械 辟 共同 运动 来 实现 的 多 种 方案 。 

如 何 协 调 移动 平台 与 机 械 臂 共同 完成 给 定 任务 ， 是 移动 作业 机 器 人 运动 规 
划 的 重点 。 有 些 研究 者 将 整个 系统 视 为 一 个 元 余 机 械 臂 ， 忽 略 移动 平台 和 机 械 
臂 两 个 子 系统 间 动 态 特性 的 差异 ， 分 别 引 入 移动 平台 和 机 械 臂 的 自由 度 ， 对 其 
进行 在 线 规划 。 参 考 文献 【50] 的 研究 没有 考虑 避 障 ， 其 研究 思路 是 : 通过 
规划 移动 平台 的 运动 使 机 械 臂 总 是 处 于 首选 区 域 ， 因 此 限制 了 移动 作业 机 器 人 
的 能 力 。 参 考 文 献 [51] 基于 Lyapunov 直接 法 ， 未 考虑 系统 动力 学 特性 ， 其 
研究 思路 是 在 实现 移动 平台 的 规划 时 ， 利 用 了 全 部 状态 的 不 连续 反馈 控制 律 和 
导航 函数 。 

高 度 非 线性 和 强 动力 学 耦合 的 多 输入 多 输出 特点 ， 决 定 了 移动 作业 机 器 人 
系统 的 动力 学 特性 直接 影响 运动 规划 。 不 同 的 规划 方法 相继 出 现 ， 为 实现 移动 
平台 的 规划 ， 采 用 基于 梯度 函数 的 分 层 合 代 算 法 进行 函数 寻 优 ， 使 机 械 臂 符合 
期 望 轨迹 给 定 条 件 [3?]。 参 考 文献 [53] 提出 的 机 械 辟 位 姿 和 移动 平台 稳定 性 
的 机 械 辟 运动 规划 方法 ， 形 成 的 最 优 规划 方法 包括 车 体 动力 学 、 机 械 辟 操作 空 
间 和 系统 稳定 性 。 参 考 文献 [54] 将 非 完 整 移动 作业 机 如 人 的 轨迹 规划 问题 
转化 为 优化 控制 问题 ， 然 后 采用 遗传 算法 或 分 级 梯度 算法 实现 了 求解 。 对 于 运 
动 规划 采用 人 工 势 场 法 ， 在 建立 移动 作业 机 器 人 的 统一 动力 学 模型 基础 上 ， 系 
统 的 渐 近 稳定 通过 DP fatti ds f ЭСЭ 557 
对 于 一 些 不 需要 考虑 两 者 同时 运动 的 情况 ， 移 动 平台 将 机 械 臂 送 到 适当 的 
范围 ， 然 后 由 机 械 辟 单独 完成 任务 ， 这 种 运动 规划 与 移动 机 器 人 的 运动 规划 相 
似 。 不 同 的 任务 对 移动 作业 机 器 人 的 要 求 不 同 ， 如 何 协 调 移动 平台 与 机 械 臂 之 
间 的 关系 是 移动 作业 机 名 人 运动 规划 的 关键 。 为 充分 发 挥 移动 作业 机 咒 人 的 能 
力 ， 移 动作 业 机 器 人 的 运动 规划 问题 有 待 国内 外 学 者 进行 进一步 的 研究 。 

(2) 移动 作业 机 顺 人 的 运动 控制 问题 ”移动 作业 机 咒 人 系统 具有 非 线性 、 
强 耦 合 的 特点 ， 其 运动 过 程 存在 着 完整 约束 或 非 完 整 约束 特性 。 其 中 移动 平台 
的 动力 学 响应 慢 ， 而 机 械 臂 的 动力 学 响应 快 。 此 外 ， 系 统 还 存在 着 模型 参数 的 
不 确定 性 问题 ， 未 建 模 动 态 及 外 界 干 扰 等 不 确定 性 的 影响 。 不 论 在 运动 学 控制 
(如 速度 控制 ) 还 是 动力 学 控制 〈 如 力矩 / 力 / 电 压 控 制 ) 方面 ， 设 计时 应 该 考 
虑 不 确定 性 因素 。 

机 融 人 的 运动 控制 几乎 与 自动 控制 同步 发 展 ， 移 动作 业 机 絮 人 的 控制 方法 
可 分 为 两 大 类 : 一 类 是 将 机 械 臂 和 移动 平台 分 为 两 部 分 处 理 的 分 散 控制 ， 对 于 
动力 学 耦合 不 重要 的 情况 ， 如 移动 平台 运动 缓慢 的 机 器 人 等 ， 可 以 忽略 两 者 间 
动态 耦合 的 影响 ， 分 别 对 两 个 系统 设计 控制 器 ; 或 者 采用 非 线 性 的 控制 方法 、 
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神经 网 络 等 方法 ， 把 系统 的 动力 学 耦合 和 未 知 不 确定 性 都 看 成 外 扰 ， 分 别 为 每 
个 子 系统 设计 合适 的 鲁 棒 控 制 吉 [5%,?7 1 。 参 考 文献 [56] 提出 的 通用 和 鲁 棒 阻尼 
控制 算法 控制 器 ， 与 系统 特性 无 关 、 针 对 系统 的 动力 学 耦合 、 参 数 和 非 参 数 不 
确定 性 的 在 线 预 信 ， 设 计 了 两 个 子 系统 的 基于 神经 网 络 的 控制 器 。 另 一 类 是 将 
机 械 臂 和 移动 平台 作为 一 个 元 余 的 机 械 臂 进行 整体 统一 控制 ， 为 保证 系统 的 稳 
定性 和 收敛 性 ， 采用 已 有 的 非 线 性 控制 方法 对 其 进行 控制 ””] 。 针 对 没有 模 
型 精确 信息 的 情况 ， 把 移动 平台 简化 为 一 个 质量 一 弹簧 一 阻尼 系统 ， 设 计 了 重 
棒 控 制 器 ， 以 实现 闭环 系统 的 稳定 3] 。 参 考 文献 [59] 对 统一 动力 学 模型 采 
用 非 线性 负 反 馈 进行 线 性 化 解 而 ， 利 用 基于 事件 的 控制 方法 实现 了 协调 控制 。 
对 于 要 求 机 械 臂 与 移动 平台 按照 期 望 的 轨迹 同时 运动 的 复杂 控制 问题 ， 为 保证 
系统 即使 受 外 界 干扰 也 能 渐 近 跟踪 给 定 信号 ， 参 考 文献 [60] 对 轮 式 移动 作 
业 机 器 人 基于 误差 动态 方程 和 耗 散 不 等 式 引 理 设计 了 重 棒 跟 踪 控 制 咒 。 人 参考 文 
献 [61] 应 用 链 式 系统 理论 并 在 考虑 了 动力 学 耦合 的 情况 下 ， 设 计 相 应 的 跟 
踪 控 制 器 。 为 保证 系统 全 部 状态 渐 近 跟踪 期 望 轨迹 ， 设 计 了 重 棒 控制 器 ， 以 解 
决 已 知 的 系统 惯性 参数 不 精确 问题 [2 。 系 统 受 到 摩擦 力 、 外 部 扰动 、 参 数 不 
确定 性 的 鲁 棒 控制 器 设计 1]。 针 对 惯性 参数 未 知 时 的 跟踪 控制 和 力 控制 问 
题 ， 设 计 的 控制 器 可 同时 解决 系统 全 部 状态 渐 近 跟踪 期 望 轨 迹 与 约束 力 渐 近 收 
敛 到 期 望 力 ![ 鸣 问题。 参考 文献 [65] 提出 了 操作 空间 中 机 械 臂 末端 惯性 有 效 
性 的 概念 ， 人 研究 了 惯性 力 对 机 械 辟 姿态 控制 影响 的 求解 方法 。 

3. 移动 作业 机 器 人 的 力 控 制 问 题 

由 于 存在 作用 端 与 外 界 接触 作用 力 (包括 力矩 ) 的 控制 要 求 ， 机 器 人 在 
受 限 空间 的 运动 控制 情况 区 别 于 在 自由 空间 的 运动 控制 情况 ， 如 何 进行 作用 力 
的 控制 ， 以 避免 机 器 人 与 工件 间 产 生 过 强 的 碰撞 而 导致 工件 变形 、 损 伤 甚至 报 
废 及 机 器 人 损坏 ， 为 了 解决 同时 对 机 咒 人 末端 位 置 和 接触 力 控制 的 问题 ， 目 前 
主要 采用 机 器 人 力 / 位 控制 的 方法 。 在 力 控 制 过 程 中 ， 要 求 机 器 人 的 运动 轨迹 
符合 期 望 的 轨迹 要 求 ， 同 时 作用 力 控制 在 规定 的 范围 内 ， 即 在 满足 运动 轨迹 的 
同时 调整 作用 力 的 大 小 。 一 种 是 采用 轨迹 控制 的 方法 ， 间 接地 保证 控制 力 的 目 
的 的 控制 方法 。 另 一 种 是 通过 在 机 器 人 上 安装 力 或 触觉 等 传感器 的 直接 力 控制 
方法 ， 机 器 人 在 受 约束 方向 上 与 外 界 间 的 作用 力 通过 传感器 可 检测 到 ， 控 制 系 
统 根据 检测 到 的 力 信 号 ， 按 照 设 定 的 控制 规律 ， 主 动 实现 对 作用 力 的 相应 控 
制 。 在 此 过 程 中 ， 为 了 使 作用 力 保持 恒 值 或 在 一 定 的 范围 内 变化 ， 机 器 人 系统 
产生 一 种 克服 作业 环境 或 者 依从 于 作业 环境 的 运动 。 在 轨迹 与 路 径 控 制 方面 ， 
区 别 于 常规 位 置 控制 絮 所 做 的 对 其 抵抗 或 消除 方式 ， 控 制 絮 对 外 界 施 加 的 作用 
力 产 生 不 同 程度 的 “妥协 ” 即 顺 应 或 依从 ， 机 器 人 对 接触 环境 顺从 的 这 种 能 
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力 称 之 为 柔顺 性 。 机 器 人 的 柔顺 性 通过 被 动 柔 顺 (Passive Compliance) 和 主动 
柔顺 (Active Compliance) 两 种 方式 实现 ， 弹 得 、 阻 尼 等 构成 的 辅助 柔顺 机 构 
是 机 器 人 被 动 柔 顺 的 主体 ， 机 器 人 在 实现 其 与 作业 环境 接触 时 ， 能 够 对 外 部 作 
用 力 产 生 自然 顺从 ; 在 根据 力 反馈 信息 实施 的 主动 柔顺 中 ， 不 同 的 控制 策略 决 
定 了 主动 控制 作用 力 的 大 小 。 主 动 柔 顺 的 控制 (Active Compliant Control) 也 
称 力 控制 (Force Control) ， 本 文 提 到 的 力 控 制 指 的 是 这 种 控制 方式 。 

机 器 人 力 /位 控制 基本 分 为 两 类 [5% 7687 . 

第 一 类 称 为 力 / 位 混合 控制 ， 既 利用 力 和 位 置 的 正 交 原理 ， 在 受 约束 方向 
上 采用 力 控制 ， 在 不 受 约束 方向 上 进行 位 置 控 制 。 对 于 作业 空间 包含 任意 方向 
的 力 和 位 置 控制 ，Raibert 和 Craig 通过 雅 可 比 矩 阵 分 配 到 各 个 关节 控制 问 上 ， 
对 于 力 与 位 置 的 控制 ， 机 器 人 以 独立 的 形式 同时 实施 ， 引 入 对 角 和 矩阵 将 力 和 位 
置 在 不 同方 向 上 分 别 进行 控制 [1 。 在 控制 器 设计 方面 ,通常 采用 常规 线性 反 
f PID, PD 控制 形式 ， 机 器 人 与 外 界 的 动力 学 特性 和 运动 学 结构 决定 了 控制 
器 的 结构 。 总 之 ， 力 /位 混合 控制 具有 理论 明确 ， 实 施 难 的 特点 。 

第 二 类 称 为 阻抗 控制 ， 为 了 达到 控制 力 的 目的 ， 通 过 建立 机 器 人 所 受 作 用 
力 与 其 位 置 (或 速度 ) 偏差 之 间 的 关系 ， 对 反馈 的 位 置 误 差 、 速 度 误差 或 刚 
度 进行 调整 "中 。 参 考 文 献 [71] 研究 了 在 笛 卡 儿 坐 标 下 把 力 反 馈 信号 转换 为 
位 置 调整 量 的 方法 ， 即 刚度 控制 。 参 考 文献 [72] 研究 了 在 笛 卡 儿 坐 标 下 把 力 
反馈 信号 转换 为 速度 修正 量 的 方法 ， 即 阻尼 控制 。Hogant ”建立 了 一 个 目标 
阻抗 的 二 阶 线性 关系 ， 把 对 力 的 控制 转化 为 对 位 置 的 控制 ， 即 阻抗 控制 。 参 考 
文献 [74] 总 结 了 阻抗 控制 的 一 般 性 与 特殊 性 ， 刚 度 控制 及 阻尼 控制 都 可 认 
为 是 阻抗 控制 的 特殊 形式 。 从 参考 文献 [75 – 84] 等 对 阻抗 控制 方法 在 算法 
简化 以 及 控制 器 设计 等 方面 所 做 的 进一步 研究 中 ， 可 以 看 出 阻抗 控制 的 控制 器 
结构 不 需要 随 着 受 限 情况 的 变化 而 改变 ， 在 自由 空间 及 受 限 空间 之 间 进 行 变 化 
时 ， 可 得 到 在 一 定 范围 内 变化 的 控制 作用 力 情况 。 在 阻抗 控制 器 设计 方面 ， 实 
现 给 定 阻 抗 关 系 式 的 前 提 ， 是 必须 先 对 机 峰 人 进行 解 厢 和 非 线 性 补偿 ， 非 线性 
补偿 控制 被 广泛 应 用 于 此 种 情况 。 阻 抗 控制 方法 在 去 飞 边 、 孔 加 工 、 刻 槽 、 抛 
光 、 折 弯 等 接触 作业 中 ， 得 到 了 一 定 的 应 用 。 

针对 上 述 两 类 力 / 位 控制 方法 ， 从 适用 范围 和 控制 效果 看 仍 有 不 足 。 为 了 
解决 机 器 人 及 负载 (包括 外 界 环境 对 象 ) 参数 不 确定 及 模型 简化 对 系统 非 线 
性 的 补偿 问题 ， 采 用 了 自 适应 控制 !5 -2] ;针对 约束 运动 ， 对 模型 参考 自 适应 
PID 控制 的 稳定 性 条 件 和 判 据 ，NicolettiG. М 用 Lyapunov 稳定 性 理论 进行 了 研 
Ж; 对 于 受 限 空间 ，Fukuda 研究 了 适应 外 界 模型 参数 自 适应 非 线性 补偿 控制 
方法 。 在 自 适 应 控制 和 和 鲁 棒 控 制 方面 ， 现 有 成 果 已 在 实际 应 用 中 取得 成 功 。 将 
自 适应 控制 用 于 可 移动 机 器 人 的 力 /位 控制 方面 ，Dong wenjie 和 Michael 的 研 
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究 成 果 具 有 代表 性 。 在 工业 机 器 人 的 力 /位 控制 中 ，Vinani、Antsaklis Roy Ж 
行 了 自 适应 控制 研究 [”-%] , Natale, Nganga - Kouya 进行 了 和 鲁 棒 控制 研 
Fy 199,100] , Natsuo Tanaka 进行 了 Hoo 自 适 应 控制 研究 !101] 。 

上 述 的 控制 方法 与 策略 中 存在 着 一 个 共同 的 建 模 难题 ， 就 是 机 器 人 本 身 具 
有 的 时 变 、 强 耦合 以 及 不 确定 性 。 此 外 ， 再 结合 力 反 馈 的 输入 ， 更 增加 了 建 模 
的 难度 。 从 现 有 的 研究 成 果 可 以 看 出 ， 智 能 力 / 位 控制 策略 的 出 现 是 必然 
的 [002 -10] 。 在 机 器 人 的 力 / 位 控制 研究 方面 ，Jasmin Velagie | 1111 All Y. Touatill21 
采用 了 模糊 控制 方法 。 在 解决 机 器 人 打 飞 边 的 问题 上 ，Hsu Feng - Yih 和 Fu Li - 
Chen 利用 了 模糊 自 适应 控制 方法 ; 在 应 用 神经 网 络 补偿 系统 淖 后 和 关节 静摩擦 
力 方 面 ，S. Jung 等 对 机 需 人 力 /位 混合 控制 使 得 性 能 易 受 影响 的 问题 进行 了 研 
究 四 年 ， 为 了 改变 系统 对 机 器 人 负载、 位 姿 、 环 境 等 效 刚 度 、 动 静摩擦 力 等 参数 
变化 的 鲁 棒 性 ， 在 经 典 力 / 位 混合 控制 右 的 力 控 制 环 里 面 ，Ferguene F 添加 了 神 
经 网 络 控制 模块 :M5] ， 范 文通 提出 由 自 适应 模糊 控制 方法 逼近 整个 受 时 变 约 束 的 
机 器 人 模型 ， 进 行 机 器 人 力 / 位 混合 控制 设计 1。 

在 改变 阻抗 控制 性 能 方面 ， 对 于 估计 环境 动力 学 模型 参数 的 研究 ， 模 糊 控 
制 、 神 经 网 络 以 及 模糊 神经 网 络 得 到 了 相应 的 应 用 。 为 了 保持 机 器 人 与 环境 间 
接触 力 稳定 ， 人 参考 文献 [117] 设计 的 模糊 控制 器 解决 了 环境 刚度 未 知 或 变化 
的 情况 ， 实 时 调整 阻抗 模型 参数 以 适应 环境 参数 变化 ; 为 减弱 阻抗 控制 中 力 控 
制 对 环境 参数 精确 度 的 要 求 ， 关 于 环境 参数 的 估计 方法 ， 参 考 文献 [118] 中 
基于 PI 控制 的 力 控制 系数 通过 采用 神经 网 络 调节 得 以 实现 ; 对 于 机 器 人 参数 
未 知 条 件 下 的 力 /位 控制 ， 针 对 阻抗 控制 参数 的 调节 ， 王 洪 瑞 等 采用 神经 网 络 
补偿 机 器 人 动力 学 模型 的 不 确定 性 ， 设 计 了 自 适 应 模糊 控制 器 [19] ; 参考 文献 
[120] 研究 了 模糊 神经 控制 和 遗传 算法 原理 补偿 环境 动力 学 参数 未 知 的 情况 ， 
以 适应 环境 变化 〈 或 未 知 ) 。 

根据 目前 的 研究 成 果 可 知 ， 智 能 控制 算法 中 的 模糊 控制 、 神 经 网 络 控制 在 
机 器 人 力 / 位 控制 的 实际 应 用 取得 了 一 定 进展 。 对 于 抛光 、 研 磨 等 实际 应 用 ， 
Amit Goradia 研究 了 环境 未 知情 况 下 ， 对 于 不 同 加 工 表 面 的 机 器 人 力 / 位 控 
制 02]。 对 于 机 器 人 力 /位 置 ，Q. P. Ha 采用 了 阻抗 控制 方法 ，B. Үао 123: 
采用 了 和 鲁 棒 自 适应 方法 。 目 前 机 器 人 力 /位 智能 控制 的 研究 还 处 于 理论 研究 阶 
段 ， 技 术 实 现 正 处 于 摸索 阶段 ， 距 离 工程 推广 实用 还 有 一 定 的 距离 5524 -27] 。 

4. 移动 作业 机 器 人 的 定位 与 建 图 研究 
智能 移动 作业 机 器 人 是 一 种 在 复杂 环境 下 工作 的 具有 自主 反应 能 力 的 机 器 
人 ， 其 目的 就 是 在 没有 人 干预 下 使 机 器 人 有 目的 地 移动 并 进行 特定 的 操作 ， 完 
成 特定 的 任务 。 自 主导 航 是 智能 移动 机 器 人 的 一 项 首要 功能 。 真 正 的 自主 导航 
要 求 机 器 人 能 够 在 一 个 完全 未 知 的 环境 中 建立 环境 地 图 ， 进 行 精准 定位 ， 以 及 
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执行 相应 的 规划 任务 。 

环境 建 模 与 定位 是 移动 作业 机 器 人 导航 领域 的 基本 问题 与 研究 热点 [28 ， 
也 是 移动 机 器 人 真正 实现 自主 的 最 重要 的 条 件 之 一 051。 所 谓 环境 建 模 
(Mapping) 是 建立 机 器 人 所 工作 环境 的 各 种 物体 如 障碍 、 路 标 等 的 准确 空间 
位 置 描述 信息 ， 即 空间 模型 或 地 图 。 而 定位 (Localization) 则 意味 着 机 器 人 必 
须 确定 自身 在 该 工作 环境 中 的 精确 位 置 。 精 确 的 环境 模型 (地图) 及 机 器 人 
定位 有 助 于 高 效 地 进行 路 径 规划 和 决策 ， 是 保证 机 器 人 自主 安全 导航 的 基础 。 
如 果 已 知 环境 的 真实 地 图 ， 估计 机 器 人 的 路 径 ， 那 就 是 定位 问题 39 。 同 样 
的 ， 如 果 机 器 人 的 真实 路 径 可 知 ， 地 图 的 创建 就 是 相对 容易 的 工作 了 55831221 。 

长 期 以 来 ， 建 图 研究 与 定位 研究 独立 进行 ， 研 究 定 位 时 需要 先 验 地 图 ， 研 
究 建 图 时 则 假定 机 器 人 位 姿 已 知 。 如 果 机 器 人 的 路 径 和 地 图 都 未 知 ， 就 必须 同 
时 考虑 定位 和 制图 ， 也 就 是 SLAM 问题 ， 即 Simultaneous Localization and Map- 
ping， 也 叫 Concurrent Mapping and Localization。 机 器 人 同时 定位 与 制图 是 移动 
作业 机 器 人 在 未 知 环境 中 行走 的 先决 条 件 ， 是 实现 其 智能 化 的 关键 ， 在 机 器 人 
领域 受到 广泛 的 关注 。SLAM 是 用 于 解决 移动 作业 机 器 人 在 未 知 环境 下 移动 的 
问题 。 机 器 人 对 自身 运动 的 状态 和 环境 中 的 目标 的 位 置 进行 观察 ， 然 后 再 重建 
一 个 精确 的 全 局 地 图 并 得 到 机 需 人 所 行走 的 路 径 ， 这 就 是 SLAM 问题 ， 它 是 移 
动作 业 机 器 人 能 和 否 真正 实现 自主 作业 的 先决 条 件 !533] 。 用 于 SLAM 研究 的 视觉 
传感器 有 多 种 配置 方式 ， 包 括 : 单 目 视 觉 、 双 目 立 体 视觉 、 多 目 立 体 视觉 以 及 
全 景 视觉 等 。 此 外 ， 视 觉 传 感 需 与 激光 、 超 声 等 传 感 需 组 合 使 用 也 受到 重视 。 

目前 ， 机 需 人 的 定位 原则 是 确定 移动 作业 机 器 人 在 运行 环境 中 相对 于 世界 
坐标 系 的 位 置 ， 移 动机 器 人 的 定位 方法 可 以 分 为 以 下 三 类 [9451 : 

І) 相对 定位 : 又 称 航 迹 推测 (Dead Reckoning, DR) ， 在 已 知 初始 位 置 
的 条 件 下 ， 通 过 利用 里 程 计 、 陀 螺 仪 等 内 部 传感器 ， 测 量 机 器 人 相对 于 初始 位 
置 的 变化 量 来 确定 当前 的 位 置 ， 具 体 可 通过 测 程 法 与 惯性 导航 方法 实现 。 对 测 
程 法 的 理解 有 狭义 和 广义 之 分 ， 利 用 编码 器 测量 轮子 位 移 增 量 推算 机 器 人 位 置 
的 狭义 测 程 法 ， 需 要 借助 外 界 传感器 的 信息 修正 编码 器 的 定位 误差 ， 通 常 采用 
卡尔 曼 滤波 法 加 以 改进 !535] 。 基 于 编码 器 和 外 界 传感器 〈 例 如 声呐 、 激 光 测 距 
仪 、 视 觉 系 统 等 ) 信息 的 广义 测 程 法 ， 对 环境 特征 信息 提取 后 与 环境 地 图 匹 
配 ， 利 用 多 传感器 信息 融合 算法 估计 机 器 人 的 位 置 137] 。 

2) 绝对 定位 : 主要 采用 导航 信 标 、 主 动 或 被 动 标识 、 地 图 匹配 或 全 球 定 
位 系统 进行 定位 。 绝 对 定位 方法 中 ， 信 和 标 或 标识 牌 的 建设 和 维护 成 本 较 高 ， 地 
图 匹配 技术 处 理 速度 较 慢 ，GPS 的 信号 易 被 遮挡 ， 只 适合 于 在 室外 开阔 的 环境 
下 定位 。 

3) 组 合 定位 : 是 针对 单一 定位 方法 的 不 足 ， 采 用 的 基于 航 迹 推测 与 绝对 
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言 息 矫 正 相 结合 的 定位 方法 。 

室内 移动 机 器 人 的 定位 技术 归纳 起 来 主要 有 : 视觉 定位 技术 、WLAN 定位 
技术 以 及 REID 定位 技术 等 。 视 觉 定 位 系统 主要 包括 : 摄像 机 或 电荷 耦合 元 件 
(charge coupled device ，CCD) ， 图 像 传 感 器 、 视 频 信号 数字 化 设备 、 基 于 数字 
信和 号 处 理 (digital signal processing, DSP) 的 快速 信号 处 理 器 、 计 算 机 及 其 外 
部 设备 等 ， 其 工作 原理 为 对 机 器 人 周边 的 环境 进行 光学 处 理 ， 完 成 机 器 人 的 自 
主导 航 定 位 功能 。 

不 论 采 用 何 种 导航 方式 ， 智 能 移动 作业 机 器 人 均 需 完成 路 径 规划 、 定 位 和 
避 障 等 任务 。 路 径 规 划 是 自主 式 移动 作业 机 器 人 导航 的 基本 环节 之 一 。 它 是 按 
照 某 一 性 能 指标 搜索 一 条 从 起 始 状 态 到 目标 状态 的 最 优 或 近似 最 优 的 无 磁 路 径 。 
根据 机 器 人 对 环境 信息 知道 的 程度 不 同 ， 可 分 为 两 种 类 型 : 环境 信息 完全 知道 的 
全 局 路 径 规 划 和 环境 信息 完全 未 知 或 部 分 未 知 ， 通 过 传 感 带 在 线 对 机 器 人 的 工作 环 
境 进 行 探测 ， 以 获取 障碍 物 的 位 置 、 形 状 和 尺寸 等 信息 的 局 部 路 径 规划 。 

基于 计算 机 视觉 技术 的 测量 是 自主 移动 作业 机 器 人 获取 环境 信息 的 主要 方 
法 ， 该 方法 是 现代 测量 技术 的 一 个 分 支 ， 是 以 现代 光学 为 基础 ， 融 合 电子 学 、 
计算 机 图 形 学 、 信 息 处 理 、 计 算 机 视觉 等 科学 技术 为 一 体 的 现代 测量 技术 。 最 
为 典型 的 是 结构 光 法 和 双 目 立体 视觉 法 。 结 构 光 三 维 测量 是 目前 在 工业 领域 中 
应 用 最 广泛 、 技 术 相 对 成 熟 的 非 接触 三 维 测量 技术 之 一 。Huang 在 参考 文献 
[138] 中 详细 介绍 了 基于 数字 条 纹 投 影 的 三 维 形 貌 测量 技术 的 最 新 进展 。 

在 获取 环境 信息 方面 ， 利 用 视觉 技术 可 实现 对 待 加 工 曲 面 模型 信息 的 获取 
及 重 构 。 实 现 自由 曲面 重 构 的 关键 技术 包括 : 数据 获取 技术 、 数 据 处 理 技 术 、 
曲线 生成 技术 、 曲 面 生 成 技术 。 数 据 获取 技术 即将 实物 模型 包含 的 表面 几何 信 
息 精 确 地 转换 成 CAD/CAM 系统 能 够 接受 的 数字 信息 。 只 有 获取 了 高 质量 的 三 
维 坐标 数据 ， 才 能 生成 准确 的 几何 模型 。 曲 线 拟 合 就 是 用 理论 曲线 去 模拟 一 组 
实验 数据 点 。 因 此 ， 曲 线 拟 合 有 两 大 任务 : 第 一 是 解决 用 什么 样 的 函数 表达 式 
去 描述 实验 数据 ， 即 确定 数学 模式 问题 ; 第 二 是 在 函数 表达 式 确定 之 后 ， 如 何 
计算 表达 式 中 的 参数 问题 。 曲 面 生成 的 方法 可 以 分 为 两 种 方式 : 一 种 是 先 由 测 
量 点 拟 合 出 组 成 曲面 的 样 条 曲线 ， 再 利用 CAD/CAM/CAE 系统 提供 的 拉 伸 、 
旋转 、 扫 掠 蒙 皮 等 功能 构造 曲面 ， 这 种 方式 较 适 合 构造 规则 曲面 或 者 曲率 变化 
平缓 的 曲面 。 另 一 种 方法 则 采用 点 云 数据 直接 拟 合 成 曲面 的 方式 ， 利 用 点 数据 
贴 合 自由 曲面 ， 以 类 似 投影 的 方式 建构 曲面 ， 拟 合算 法 可 以 是 基本 的 盘 近 法 、 
插 补 法 或 二 者 的 结合 。 
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对 于 大 型 模具 曲面 的 研磨 加 工 ， 如 果 采 用 传统 的 加 工 方式 ， 就 需要 研制 大 
型 的 研磨 设备 ， 但 是 研发 与 制造 大 型 研磨 设备 的 成 本 高 、 难 度 大 。 另 外 研磨 设 
备 无 法 适应 不 同 尺 寸 工件 的 变化 需求 。 结 合 目前 移动 作业 机 器 人 的 研究 成 果 ， 
本 文 介绍 一 种 具有 实用 性 的 研磨 大 型 模具 自由 曲面 的 移动 作业 机 器 人 系统 。 

该 系统 是 基于 x — y 移动 平台 形式 的 吸盘 式 机 器 人 人， 适合 于 在 变化 的 空间 


8+4 d ume 操作 臂 可 以 实现 符合 研磨 性 能 要 求 的 位 姿 ， 同 时 研磨 工具 头 具 
有 符合 控制 要 求 的 可 操纵 性 ， 本 章 对 机 器 人 的 机 械 结 构 、 运 动能 力 和 集成 系统 
odd 


2.1 移动 研磨 作业 机 器 人 系统 


2.1.1 上 自主 作业 研磨 机 器 人 系统 技术 思 


本 文 介 绍 的 大 型 曲面 研磨 机 器 人 系统 由 研磨 机 器 人 本 体 、 控 制 系统 和 曲面 
模型 重 构 与 定位 系统 构成 。 

研磨 机 器 人 本 体 : 是 完成 机 器 人 加 工 运 动 的 主体 部 分 ， 包 括 构成 机 器 人 的 
移动 平台 和 操作 臂 机 构 、 研 磨 工具 部 分 及 相应 的 驱动 电动 机 、 气 动 系统 和 各 种 
机 载 的 电 、 气 元 件 。 

控制 系统 : 用 于 执行 机 器 人 在 研磨 加 工 和 运动 中 需要 的 各 种 计算 、 决 策 及 
对 执行 元 件 的 控制 。 人 硬件 部 分 包括 用 于 机 带 人 控制 计算 的 计算 机 、 用 于 控制 各 
轴 运 动 的 多 轴 运 动 控 制 卡 以 及 各 种 配套 的 元 件 。 软 件 部 分 包括 曲面 重 构 模块 、 
位 置 控制 模块 、 力 控制 模块 ， 根 据 控制 结构 的 功能 要 求 ， 控 制 系统 软件 分 为 三 
层 : 上 层 根据 机 器 人 运动 学 、 动 力学 模型 对 机 器 人 的 导航 和 其 他 控制 进行 决 
策 ; 中 间 层 对 机 器 人 的 速度 、 加 速度 及 位 置 进行 控制 和 分 配 ， 下 层 完 成 对 机 屁 
人 执行 机 构 的 控制 ， 如 移动 平台 的 运动 、 各 关节 的 驱动 等 。 

曲面 重 构 与 定位 导航 系统 : 完成 对 待 加 工 曲面 区 域 的 模型 重 构 与 定位 ， 是 
整个 系统 的 基础 工作 部 分 。 可 实现 待 加 工 曲 面 模型 的 重 构 ， 然 后 通过 控制 系统 
进行 机 器 人 运动 的 位 置 、 姿 态 坐 标 检测 和 计算 。 
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2.1.2 自主 作业 研磨 机 器 人 的 结构 与 性 能 要 求 


根据 机 器 人 研磨 大 型 模具 自由 曲面 的 工艺 要 求 ， 研 发 的 研磨 移动 机 器 人 技 
术 要 求 如 下 : 

1. 机 器 人 工作 环境 要 

1) 机 器 人 工作 环境 为 室内 或 室外 。 

2) 大 型 自由 曲面 的 曲率 较 平缓 ， 曲 面 规格 应 大 于 lm x Im, 

3) 机 器 人 工作 环境 温度 应 在 10 ~40%C 。 

2. 机 器 人 主要 技术 指标 要 求 

1) 机 器 人 整体 尺寸 : 小 于 600mm x 600mm, 

2) 机 器 人 本 体质 量 : 小 于 60kg。 

3) ЛАТА: 0 ~ 1. 5m/min, 

4) 机 器 人 运动 要 求 : 平稳 、 灵 活 和 较 好 的 可 操纵 性 ， 可 以 实时 调整 变换 
位 置 和 姿态 。 


2.2 上 自主 作业 研磨 机 器 人 机 械 本 体 的 研制 


我 们 研制 的 机 器 人 整体 结构 如 图 2-1 所 示 ， 和 运动 顺序 为 : 从 机 器 人 坐标 系 
的 原点 开始 ， 沿 着 第 一 个 关节 运动 到 末端 关节 ， 移 动 副 用 T 了 表示， 转动 副 用 R 
表示 ， 本 文 的 研磨 机 器 人 可 命名 为 5 -TTRRT 机 器 人 。5 -TTRRT 机 器 人 分 为 
全 方位 移动 平台 与 操作 臂 两 部 分 。 其 中 移动 平台 上 安装 有 直角 坐标 机 构 、 四 足 
伸缩 电动 推 枉 机 构 、 四 个 吸盘 模块 、 单 目 视觉 摄像 机 、 激 光 装 置 和 倾斜 计 等 。 


2.2.1 移动 平台 的 构建 


我 们 研制 的 5 — TTRRT 机 器 人 移动 与 转向 机 构 是 由 一 个 直角 坐标 机 构 来 实 
现 的 ， 该 机 构 由 两 个 相互 垂直 的 直线 运动 轴 组 成 ,整体 的 框架 结构 采用 的 是 滚 
珠 丝 杠 配 合 滑动 导轨 的 形式 ， 两 个 运动 轴 对 应 直角 坐标 系 中 的 x 轴 和 y 轴 ， 实 
现在 水 平面 内 的 运动 。 由 步 进 电动 机 分 别 驱动 x 轴 或 y 轴 的 滚珠 丝 杠 副 工 作 ， 
实现 沿 着 x 轴 或 y 轴 的 运动 。 通 过 插 补 算法 控制 步 进 电 动机 分 别 实现 x 轴 或 y 
轴 的 直线 运动 方式 ， 以 控制 移动 平台 到 达 工 作 位 置 。 

为 实现 移动 平台 在 任何 位 置 都 能 可 靠 工 作 ， 采 用 了 带 有 四 个 可 伸缩 足 腿 的 
方案 ， е s 电动 
推 杆 伸 出 的 最 大 行程 为 50mm。 在 电动 推 杆 的 工作 端 安装 了 真空 吸盘 ,为 了 保 
证 移动 平台 平行 于 水 平面 ， со оо ооо T. 
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略 。 利 用 装 在 机 器 人 本 体 上 的 姿态 传感器 一 倾斜 计 ， 可 以 实时 地 测 出 机 器 人 
x -yy 平面 与 铅 垂 线 之 间 的 夹 角 ， 通 过 该 夹 角 ， 可 以 求 出 移动 平台 姿态 变化 的 程 
度 。 将 倾斜 计 输 出 的 电压 信号 通过 运动 控制 卡 上 A/D 口 转换 成 数字 量 ， 最 后 
送 入 计算 机 以 实现 机 器 人 本 体 的 姿态 控制 。 

















42-1 自主 移动 研磨 机 器 人 的 结构 图 
1 一 x 向 进 给 机 构 2—у 向 进 给 机 构  3—z 向 进 给 机 构 “4 一 摆动 机 构 
5 一 转动 机 构 “6 一 激光 装置 “7 一 摄像 机 





























2.2.2 操作 臂 的 研制 


根据 研磨 的 工作 特性 与 要 求 ， 在 研磨 过 程 中 ， 无 论 是 采用 手工 研磨 还 是 机 
上 咒 研磨 ， 都 要 求 研磨 工具 以 一 定 的 姿态 作用 于 被 加 工 的 表面 上 ， 并 沿 着 研 麻 轨 
迹 对 表面 进行 加 工 ， 即 研磨 工具 与 待 加 工 表 面 存在 一 定 的 相对 位 置 关 系 ， 在 这 
一 过 程 中 ， 操 作者 通常 通过 改变 法 向 研磨 力 人 研磨 工具 的 线 速度 vw、 进 给 速 
度 f 及 姿态 角 B 等 工艺 参数 来 获得 理想 的 研磨 效果 。 由 于 研磨 工具 与 自由 曲面 
之 间 的 接触 为 三 维 接触 ， 为 了 保证 研磨 作用 力 沿 着 自由 曲面 上 的 法 线 方向 施 
加 ， 避 免 研磨 工具 与 自由 曲面 之 间 发 生 干 处， 工作 过 程 中 研磨 工具 的 姿态 要 根 
据 需 要 发 生 相应 的 改变 。 研 磨 接触 区 域 的 形状 和 尺寸 以 及 研磨 接触 作用 力也 随 
着 研磨 工具 姿态 和 上 自由 曲面 表面 曲率 的 变化 而 变化 。 根 据 常用 的 研磨 工具 形状 
"DAL, EA. BUR. 、 螺 旋 线 等 简单 曲线 及 其 组 合 构成 了 研磨 工具 的 工作 部 分 。 
常用 的 工具 为 及 其 组 合 形式 可 分 为 : 平底 工具 ， 柱 形 工具 ， 锥 形 工具 ， 鼓 形 工 
有 具 ， 球 面 工具 ， 圆 环 面 工 具 ， 反 圆 环 面 工具 等 。 根 据 研 磨 工 具 包 络 截 型 的 曲率 
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分 布 ， 还 可 分 为 不 可 变 的 、 可 变 的 、 可 大 幅度 改变 的 等 几 种 类 型 的 工具 。 自 由 
曲面 的 曲率 变化 相差 很 大 ， 在 对 曲面 进行 加 工时 ， 工 具 包 络 截 型 的 曲率 分 布 情 
况 及 其 变化 幅度 是 影响 加 工 的 主要 因素 。 

本 文 介绍 的 5 — TTRRT 研磨 机 器 人 主要 针对 的 是 采用 球 头 研磨 工具 进行 加 
工 的 情况 。 因 此 ， 通 过 研磨 工具 的 工具 轴 矢 量 与 工件 被 加 工 表面 的 法 线 方向 要 
保持 一 定 的 姿态 角 ， 并 保证 研磨 作用 力 沿 着 自由 曲面 上 的 法 线 方向 施加 。 此 
外 ， 研 磨 工 具 的 运动 方向 与 被 加 工 曲 面 上 不 同 曲率 的 对 应 点 切 向 方向 一 臻 也 是 
需要 保证 的 。 

研磨 机 器 人 的 操作 臂 由 三 个 关节 组 成 ， 绕 y 轴 方 向 的 转动 关节 D EE 
在 与 y 轴 方向 固 联 的 框架 上 。 实 现 绕 x* 轴 方 向 转动 的 摆动 关节 卫 ， 通 过 齿 
轮 齿 条 机 构 安装 在 转动 关节 工 上 。 实 现 直线 运动 的 关节 亚 ， 与 摆动 关节 工 
[5] EK. 

转动 关节 I 由 步 进 电动 机 驱动 绕 y 轴 方 向 转动 ， 转 动 范围 为 30° ~ 30°, 

EDA П Н 2268 LK 36 ALY LE, CVE, Aree ae 
Taie 3 Ж ту 5 I TL AE, 运动 的 中 心平 行 于 % 轴 方 向 ,运动 范围 
为 -30° ~30°, 

直线 运动 关节 亚 的 滚珠 丝 杠 部 分 与 摆动 关节 工 固 联 ， 通 过 滚珠 丝 杠 机 构 的 
螺母 带动 研磨 工具 头 进行 工作 。 

5 - TTRRT 机 器 人 机 械 实 物 如 图 2-2 所 示 。 
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2.2.3 ”研磨 工具 开发 


利用 现 有 的 电动 工具 如 图 2-3 所 示 ， 在 电动 工具 与 直线 运动 关节 耳 的 连接 
处 ， 安 装 的 研磨 工具 机 构 含 有 压力 传感器 与 被 动 柔 顺 环 节 (弹簧)， 如 图 2-4 
所 示 。 可 由 压力 传感器 测 得 实际 接触 力 ， 通 过 基于 位 置 的 阻抗 控制 以 实现 机 咒 
人 的 柔顺 控制 。 











图 2-3 ”研磨 工具 













































































































































































































































































































































































图 2-4 研磨 工具 机 构 示 意图 


2.3 上 日 主 作业 研磨 机 占 人 控制 系统 


本 文 介绍 的 5 - TTRRT 研磨 机 器 人 控制 系统 的 主体 采用 “PC + 运动 控制 
卡 ” 的 主 从 式 二 级 控制 ， 来 实现 机 器 人 工具 头 终端 的 位 姿 控 制 和 运动 控制 。 总 
体 结构 如 图 2-5 所 示 。 
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图 2-5 控制 系统 结构 





2.4 5-ТТЕКТ 机 器 人 坐标 系 内 研磨 曲面 的 构建 方法 


为 了 保证 5 - TTRRT 机 器 人 能 够 准确 地 进行 曲面 的 研磨 工作 ， 首 先 必须 建 
立 待 研 磨 曲面 在 机 器 人 坐标 系 中 的 三 维 模型 ， 然 后 再 根据 三 维 曲面 模型 进行 
5 -TITRRT 机 器 人 的 运动 规划 和 速度 规划 ， 进 行 这 些 工 作 的 前 提 是 必须 要 对 待 
加 工区 域 建立 曲面 模型 。 

目前 ， 自 由 曲面 的 三 维 重 构 方法 主要 分 为 接触 式 和 非 接触 式 两 大 类 : 接触 
式 重 构 应 用 探 针对 表面 进行 接触 ， 较 为 可 靠 但 容易 引起 工件 变形 、 划 伤 和 较 大 
的 测量 误差 ， 而 且 测 量 速度 较 慢 ' , 非 接 触 式 重 构 分 为 主动 结构 光 法 和 立体 
视觉 法 等 ， 原 理 较为 简单 ， 但 这 些 方法 一 般 需 要 两 台 和 多 台 摄 像 机 ， 价 格 昂贵 
MERRNI], EPELERA, RAHATA H А те А0 27 
法 ， 以 实现 机 器 人 工作 空间 内 待 加 工 曲 面 的 三 维 重 构 。 首 先 利用 单 目 摄像 机 和 
一 字 线 激光 需 构 造 了 一 套 视 觉 采集 系统 ， 获 取 磨 削 机 器 人 竺 加工 表面 的 序列 激 
光线 图 片 ; 然后 根据 激光 线 特点 ， 采 用 最 小 二 乘法 的 二 次 抛物 线 拟 合 的 方法 对 
每 幅 图 片 中 的 激光 线 中 心 进行 提取 ; 对 存在 的 间断 激光 线 ， 利 用 三 次 B 样 条 
曲线 算法 对 每 幅 图 片 的 间断 线 进行 拟 和 生成 一 条 光滑 连续 的 曲线 ， 最 后 将 每 幅 
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图 片 的 激光 线 的 强度 信息 拼接 在 一 起 ， 构 成 完整 的 机 器 人 坐标 系 内 待 加 工 表面 
的 三 维 信息 图 片 ， 进 而 进行 机 器 人 的 运动 规划 。 


2.4.1 系统 组 成 和 工作 原理 


依据 摄影 测量 理论 的 结构 光 三 维 测量 法 ， 有 效 地 利用 可 控 光 源 和 图 像 处 理 
相关 的 测量 技术 ， 可 以 在 一 些 特殊 情况 下 改善 三 维 坐 标 测量 精度 。 例 如 对 于 一 
些 表面 较 光 滑 、 形 状 变化 较 大 、 缺 乏 纹理 、 灰 度 不 明显 的 表面 区 域 ， 可 以 在 物 
体 表 面 上 形成 明显 的 结构 光 条 纹 ， 很 容易 在 信息 贫乏 区 域 找到 相应 的 匹配 
点 02] 。 结 构 光 源 加 单 相机 的 立体 测量 系统 在 三 维 重 构 方面 应 用 较为 普遍 ， 本 
文 介 绍 的 自动 测量 系统 ， 也 是 利用 这 种 典型 的 硬件 结构 来 实现 磨 削 的 大 型 自由 
曲面 的 三 维 重 构 。 

曲面 重 构 系统 的 组 成 原理 如 图 2-6 所 示 ， 在 该 系统 中 ， 传 统 接触 式 坐 标 测 
量 机 的 接触 式 测 头 被 单 摄像 机 和 线 结构 光 替 代 了 。 将 线 激 光 融 固定 到 机 器 人 坐 
标 系 的 z 轴 方 向 上 ， 其 光 轴 与 z 轴 方向 平行 ， 图 像 平面 的 坐标 * П у 轴 分 别 
平行 于 机 器 人 坐标 系 的 x 轴 和 y 轴 。 在 机 器 人 对 竺 加工 表面 的 曲面 进行 三 维 重 
构 时 ， 首 先 机 器 人 进入 待 研磨 区 域 ， 然 后 机 器 人 沿 x 轴 方 向 做 水 平 运动 ， 同 时 
打开 线 激光 器 和 摄像 机 开始 进行 图 像 采 集 ， 得 一 系列 图 像 ， 当 机 天 人 运动 到 x 
轴 的 极限 位 置 后 ， 关 掉 线 激光 器 和 摄像 机 ， 机 器 人 回 到 原 位 ， 同 时 曲面 重 构 系 
统 对 已 扫描 的 曲面 进行 三 维 重 构 。 如 果 利 用 这 些 图 像 序列 重 构 一 个 三 维 图 像 ， 
首先 需要 利用 调 焦 评价 函数 在 这 些 图 像 序 列 中 找到 图 像 上 每 一 个 像素 的 正 焦 
点 ， 根 据 这 些 正 焦点 获得 自由 曲面 的 正 焦 图 像 ， 再 利用 物 像 关 系 式 得 到 当前 视 












































а) 单 摄像 机 与 激光 布置 示意 图 b) 摄像 机 安装 位 置 示意 图 
图 2-6 看 重 构 系 统 的 组 成 原理 图 
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场 的 自由 曲面 上 点 的 三 维 坐 标 值 。5 - TTRRT 机 器 人 带动 摄像 机 和 线 激 光 器 在 
被 测 工件 表面 循环 扫描 、 计 算 ， 最 终 得 到 全 部 自由 曲面 上 点 的 三 维 坐 标 。 

研磨 机 器 人 自由 曲面 三 维 重 构 的 硬件 系统 构成 : 图像 采集 为 北京 大 恒 
DH - HV1302UM 的 分 辨 率 为 1280 x 1024 的 摄像 机 ， 激 光 器 为 M635ALS - 24 
一 字 线 激光 器 。 在 图 像 采 集 系 统 的 安装 过 程 中 ， 确 保 相机 镜头 与 z 轴 的 夹 角 能 
够 清晰 地 获得 每 帧 图 像 上 完整 的 激光 线 ， 并 且 尽 量 保证 摄像 机 与 z 轴 的 夹 角 足 
够 大 ， 使 得 曲面 在 z 轴 方 向 较 小 的 高 度 变化 就 可 以 获得 激光 线 在 图 像 上 较 大 范 
围 的 变化 。 同 样 的 高 度 ， 激 光线 变化 范围 越 大 ， 所 获取 的 曲面 模型 的 精度 
越 高 。 
2.4.2 自由 曲面 的 三 维 重 构 

1. 激光 线 的 提取 

曲面 重 构 就 是 根据 曲面 实物 模型 的 点 云 ， 重 建 其 几何 和 拓扑 信息 ， 并 再 现 
其 特征 的 过 程 03] 。 本 文采 用 抛物 线 拟 合 法 实现 了 图 像 中 激光 线 中 心 的 提取 ， 
其 中 线 激 光源 发 射 在 被 测 物体 表面 形成 的 激光 条 在 宽度 方向 上 其 激光 的 强度 近 
似 高 斯 分 布 。 图 2-7a 为 本 实验 中 激光 线 的 一 个 截面 分 析 图 。 基 于 其 激光 的 强 
度 近 似 高 斯 分 布 这 一 特点 ， 本 文 利 用 最 小 二 乘法 的 二 次 抛物 线 拟 合法 提取 激光 
线 中 心 的 坐标 041 。 
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а) 激光 线 光 强度 截面 图 b) 一 次 抛物 线 拟 合 


图 2-7 激光 线 截面 图 与 中 心 的 提取 





利用 最 小 二 乘法 的 二 次 抛物 线 拟 合法 提取 激光 线 中 心 的 数学 原理 : A т 
组 实验 数据 (x,, ур), k=1, 2, s, m, 可 以 求 取 抛 物 线 的 系数 ay, 


а], ао 
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р(х) =ау+аух + азж? (2-1) 
使 其 满足 
5(а,ау,а) =р(х) - y, = min (2-2) 
TEX (2-2) 中 ， 为 了 使 $ 满足 最 小 值 ， 则 有 
ds/da; =0 (j=0,1,2) (2-3) 
从 而 可 以 得 到 
т b Y x > у, 
k=1 k=l Е=1 
а 
У x, > > < x " У ху (2-4) 
Е=1 k=l k=l Е=1 
т m m a m 
Ул Lu L4 У, ху, 
Е=1 k=1 k=l k=1 
由 式 (2-4) 可 以 得 到 参数 a, ау, ，a,。 其 极 大 值 位 置 为 
Xmax = — a,/( 2a ) (2-5) 


利用 抛物 线 拟 合法 求 取 光 条 中 心 的 算法 原理 : ТЕШ EUR FH CERE B ETE 
(otsu) 1S 获得 最 佳 阅 值 7， 从 左 至 右 逐 列 扫描 预 处 理 后 的 光 条 并 求 得 每 一 列 
的 灰 度 最 大 值 ， 这 些 最 大 值 的 点 ， 其 值 如 果 小 于 最 佳 冰 值 7， 则 判断 其 是 光 条 
外 的 点 ; 其 值 如 果 大 于 最 佳 阔 值 TY， 则 判断 其 为 光 条 上 的 点 。 这 时 在 点 的 上 下 
两 侧 分 别 取 个 像素 点 (因为 2n +1 是 光 条 横 截面 宽度 ) ; 将 这 些 2n +1 个 像 
素 点 的 灰 度 值 视 为 抛物 线 上 的 点 ， 通 过 求 取 抛物 线 上 的 极 大 值 获得 该 列 的 激光 
线 光 条 中 心 ， 见 图 2-7b。 

以 某 汽车 覆盖 件 进 行 表面 重 构 过 程 为 例 ， 说 明 激光 线 的 提取 过 程 。 图 2-8a 
为 对 某 一 汽车 覆盖 件 图 像 采 集 过 程 中 获得 的 序列 图 像 之 一 ， 在 此 图 像 中 有 一 条 
非常 明显 的 激光 线 ， 通 过 对 图 像 进行 处 理 ， 可 以 从 图 像 中 提取 出 激光 线 信息 ， 
生成 深度 图 ， 图 2-8b 为 采用 抛物 线 拟 合法 获得 的 光 条 中 心 的 激光 线 图 像 。 

为 检验 这 种 抛物 线 拟 合法 求 光 条 中 心 方法 的 效果 ， 用 计算 多 次 测量 的 算术 
平均 值 的 标准 差 方 法 ， 对 某 一 空间 点 的 像素 坐标 重复 测量 n 次 ,计算 该 像素 点 
的 算术 平均 值 的 标准 差 ， 对 系统 测量 空间 中 m 个 空间 点 的 算术 平均 值 的 标准 
25, 计算 其 均 方 根 作为 整体 的 标准 差 。 在 中 心 拟 合 实 验 中 任意 选取 一 条 光 条 的 
像素 数 m =250; 每 个 空间 点 重复 测量 10 HK, 即 n=10。 表 2-1 中 为 其 中 一 小 
段 像素 个 数 为 10 的 激光 线 用 拟 合 抛物 线 方法 计算 得 到 的 中 心 值 的 算术 平均 值 
和 10 次 重复 测量 的 标准 差 ， 整 体 的 标准 差 为 0.018 像素 。 
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а) 原始 图 像 


b) 提取 后 的 激光 线 





图 2-8 抛物 线 拟 合法 求 光 条 中 心 




















表 2-1 光 条 中 心算 术 平 均值 及 其 标准 差 像素 
序号 光 条 中 心 y 标准 差 a 序号 ЖЖ y 标准 差 o 
1 258. 43 0.012 6 257. 11 0.018 
2 258. 02 0. 015 7 256. 74 0. 018 
3 258. 15 0. 021 8 256. 62 0.012 
4 257. 83 0. 008 9 256. 30 0. 000 
5 251.51 0. 017 10 256. 20 0. 006 



























































通过 线 激光 图 像 法 得 到 图 像 ， 计 算出 激光 线 中 心 坐标 ， 然 后 根据 已 建立 的 
视觉 检测 数学 模型 及 二 维 图 像 坐标 ， 通 


过 后 面 介绍 的 三 维 曲 面 的 重 构 方 法 得 到 
对 应 空间 点 的 坐标 ， 经 检验 整个 测量 系统 的 测量 精度 可 以 达到 0. 079mm。 
2. 填充 丢失 的 数据 点 与 三 次 B 样 条 曲线 





= 





通过 一 系列 图 像 预 处 理 后 得 到 的 一 字 激 光线 的 图 像 如 图 2-9 所 示 ， 光 线 在 


纵向 上 的 变化 反映 了 实际 加 工 表 面 上 高 度 的 变化 。 从 图 中 可 以 看 出 ， 这 些 提取 
的 激光 线 是 间断 的 ， 这 样 将 失去 部 分 曲面 信息 ， 这 种 情况 对 于 机 器 人 的 曲面 研 
磨 加 工 就 会 存在 一 些 加 工 不 到 的 区 域 。 机 顺 人 的 研磨 加 工 是 在 工件 的 曲面 半 精 
加 工 完 成 以 后 进行 的 ， 所 以 它 的 曲面 表面 应 该 是 较为 光滑 连续 的 。 因 此 ， 对 于 
这 些 间 断 的 激光 线 ， 可 以 应 用 三 次 B 样 条 曲线 算法 对 这 些 点 进行 拟 合 。B 样 条 








曲线 具有 很 好 的 局 部 控制 性 。 对 于 В 样 条 曲线 ， 多 项 式 的 次 数 不 取 决 于 特征 








多 边 形 控制 顶点 的 数目 ， 避 免 了 B 样 条 曲线 次 数 随 着 控制 顶点 数目 的 增加 而 


增 大 的 缺点 。 这 样 激光 线 的 间断 点 的 




















由 线 拟 合 只 和 它 相 邻 的 几 个 点 有 关联 。 


基于 特征 点 的 位 置 矢量 和 Bézier 方法 ， 是 自由 曲线 构造 的 理论 基础 。 就 其 
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а) 间断 激光 线 图 b) 局 部 放大 图 


图 2-9 提取 的 一 字 激 光线 图 片 








本 质 而 言 š 由 多 段 Bézier 曲线 构成 的 B 样 条 曲线 ， 是 Bézier 曲线 段 的 集合 。 

п -2 条 三 次 多 项 式 曲 线段 (Bezier 曲线 段 ) r (u), r (u), с, r, 3 Сш) WJ BÑ. 
的 三 次 B 样 条 曲线 是 n+1 T BERE Т да Bus r Ж, =I B 样 条 曲线 的 数学 表 
达 为 [146] 











-1 3 -3 1 es 
1 3 -6 3 0 Tj-2 

r;(u) =r;(v) = tu v v 1)x PF dea x - (0zxvzxl) 
1 4 1 0 r 


(2-6) 

st (2-6) 中 ,ry(w) 为 B 样 条 曲线 上 任 一 点 的 位 置 失 量 。 图 2-10 是 使 用 
3 次 均匀 B 样 条 曲线 处 理 后 的 激光 线 图 片 ， 从 图 中 可 以 看 出 ， 此 时 的 激光 线 是 
非常 光滑 连续 的 。 




















图 2-10 3 次 均匀 B 样 条 曲线 处 理 后 的 激光 线 
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把 层 断 面 上 的 点 拟 合成 非 均 匀 有 理 B 样 条 (NURBS) 曲线 ,通常 有 三 种 
方法 : 以 线段 趋 近 点 群 来 连接 线段 ， 线 段 并 不 一 定 通过 每 一 个 点 ， 但 线段 比较 
平滑 ; 将 点 群 中 的 每 一 个 点 依照 顺序 串联 起 来 ， 生 成 的 曲线 通过 每 一 个 点 ; DR 
证 一 定 的 公差 范围 的 方法 。 本 例 中 采用 第 三 种 方法 ， 以 保证 曲面 的 精度 ， 生 成 
的 曲线 如 图 2- 10。 

3. 曲面 重 构 实验 

对 于 拍摄 到 的 序列 图 像 中 的 每 张 图 片 ， 都 可 以 采用 上 述 的 3 R В В 
条 曲线 算法 进行 处 理 ， 进 而 可 得 到 一 条 光滑 的 曲线 ， 曲 线 上 每 点 的 了 值 变 化 
代表 曲面 上 对 应 点 的 高 度 变 化 ， 它 体现 在 整个 工件 图 像 中 的 每 点 的 灰 度 值 不 
同 。 将 所 有 的 序列 图 像 得 到 的 激光 线 拼 接 成 一 幅 完 整 的 图 片 就 是 重 构 后 的 图 
片 ， 见 图 2-11。 











a) 灰 度 图 像 b) 3D 图 像 
Al2-11 待 加 工 表面 重 构 后 的 图 片 





图 2-11 所 示 就 是 待 研磨 表面 的 曲面 三 维 模型 ， 在 此 基础 上 ， 通 过 重 构 后 
的 曲面 对 应 边界 点 的 数值 ， 就 能 得 到 重 构 后 的 竺 加工 曲面 在 机 器 人 坐标 系 中 的 
坐标 ， 它 是 机 器 人 轨迹 规划 和 运动 规划 的 基础 。 

将 结构 光 图 像 法 测量 结果 和 三 维 坐 标 测量 仪 测量 结果 进行 比较 ， 结 果 见 表 
2-2。 从 表 2-2 数据 来 看 ， 这 种 基于 线 结构 光 图 像 法 的 测量 方法 在 速度 、 成 本 
方面 具有 很 大 优势 。( 注 : 结构 光 图 像 法 的 测量 精度 是 依据 三 坐标 测量 仪 测量 
结果 计算 出 来 的 。) 














表 2-2 实验 结果 及 误差 分 析 
测量 方式 测量 精度 /mm 方差 测量 时 间 成 本 
线 结构 光 图 像 法 0.08 0. 0416 3s 较 低 
三 坐标 测量 仪 0. 0005 一 20min 
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整个 研磨 机 器 人 工作 的 流程 如 图 2-12 所 示 。 


CPU 读 取 所 采集 的 
曲面 AVI 文件 





读 取 一 张 
序列 图 片 


生成 连续 光滑 激光 
线 ， 存 入 缓冲 区 中 


机 器 人 运动 到 指定 区 
Ж 


机 器 人 沿 x 轴 方向 返 
动 ， 同 时 打开 激光 器 
和 摄像 机 


机 器 人 运动 到 x 负极 
限 位 ， 同 时 关闭 激光 
器 和 氢 像 机 


读 取 AVI 结 束 ? 


йлн Ах 25 Va Ind 
到 原 位 


将 所 有 缓冲 区 中 光滑 
的 激光 线 拼接 成 CR 
完整 的 出面 灰 度 网 



































根据 生成 的 灰 度 图 ， 
进行 机 器 人 的 运动 
利和 轨迹 规划 

















命令 机 器 人 开始 对 
面 进行 磨 削 














图 2-12 研磨 曲面 的 建立 、 定 位 及 工作 流程 
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针对 已 知 或 未 知 曲面 模型 ， 本 文 的 5 -TITRRT 机 器 人 研磨 自由 曲面 时 的 运 
动 规划 技术 包括 : 轨迹 规划 与 路 径 规划 。 移 动 平台 运动 时 ， 通 常 需要 知道 机 器 
人 的 运动 路 径 或 路 径 上 的 离散 点 ， 通 过 轨迹 规划 可 求解 研磨 机 器 人 运动 过 程 中 
关节 或 末端 执行 器 各 时 刻 的 位 置 、 速 度 和 加 速度 。 机 器 人 系统 工作 时 首先 要 确 
定 其 运动 轨迹 ， 根 据 要 完成 某 种 作业 的 目标 ， 对 其 运动 轨迹 进行 规划 ， 轨 迹 规 
划 决 定 了 系统 的 工作 方式 和 效率 ， 轨 迹 规划 可 以 在 机 器 人 关节 空间 或 者 笛 卡 儿 
空间 中 进行 ;在 进行 自由 曲面 研磨 精 加 工作 业 时 ， 要 保证 机 器 人 跟踪 期 望 轨迹 
运动 。 运 动 (或 轨迹 ) 规划 和 轨迹 跟踪 控制 是 本 文 的 5 -TIRRT 机 器 人 运动 
控制 的 两 方面 内 容 ， 在 关节 变量 空间 的 规划 中 ， 描 述 机 器 人 的 预定 任务 通过 规 
划 关 节 变 量 的 时 间 函 数 来 实现 ， 方 法 简单 ， 不 会 产生 奇异 位 置 ; 在 笛 卡 儿 空 间 
规划 时 ， 需 要 建立 机 器 人 工具 末端 位 置 对 应 的 时 间 函 数 ， 通 过 机 器 人 的 逆 运 动 
学 可 求解 出 相应 不 同 关 节 的 位 置 。 本 文 的 5 -TITRRT 机 器 人 在 工作 过 程 中 ， 必 
须 保证 研磨 工具 末端 按 预定 路 径 运动 ， 同 时 又 要 避免 干涉 ， 必 须 绕 过 障碍 物 ， 
本 文选 择 在 笛 卡 儿 空 间 进 行规 划 。 

本 文中 的 机 器 人 机 械 臂 作业 过 程 可 分 解 为 在 自由 空间 的 运动 过 程 和 约束 空 
间 的 运动 过 程 。 在 自由 空间 运动 阶段 ， 为 提高 工作 效率 ， 以 作业 时 间 最 短 为 目 
标 ， 实 施 机 器 人 的 自由 运动 轨迹 规划 技术 。 在 约束 空间 运动 阶段 ， 根 据 待 加 工 
工件 的 制造 特征 ， 机 器 人 约束 运动 路 径 规 划 技 术 与 工件 的 制造 特征 相映 射 。 将 
工件 的 设计 特征 映射 为 制造 特征 ， 进 而 引导 机 器 人 进行 作业 路 径 跟 踪 ， 机 器 人 
的 位 置 求解 通过 逆 运 动 学 分 析 的 结果 得 到 。 

自由 曲面 是 由 不 同类 型 的 曲面 组 合 而 成 的 形状 复杂 的 复合 曲面 ， 对 于 简单 
解析 曲面 的 数学 模型 ， 可 通过 简单 的 非 参 数 数学 解析 式 表 达 。 对 于 自由 曲面 的 
数学 模型 ， 需 要 建立 相应 的 参数 方程 式 来 表达 。 所 以 无 法 直接 用 数学 表达 式 实 
现 自由 曲面 的 数学 模型 表达 结果 。 本 文 的 5 - TTRRT 机 器 人 系统 进行 运动 规划 
时 ,需要 结合 上 一 章 获 取 的 待 加 工 自 由 曲面 模型 数据 ， 获 得 研磨 工具 的 刀 位 点 
及 加 工行 距 ， 然 后 确定 相应 的 加 工 轨迹 。 因 此 ， 本 文 首先 对 待 加 工 的 自由 曲面 
进行 分 片 划分 ， 然 后 针对 划分 后 的 曲面 选择 相应 的 加 工 路 径 规划 方法 ， 作 为 进 
一 步 建 立 机 器 人 运动 控制 策略 的 基础 。 
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3.1 5- TTRRT 机 器 人 运动 学 与 动力 学 模型 


5 - TTRRT 研磨 机 器 人 为 5 - DOF 机 器 人 ， 其 中 移动 平台 有 两 个 自由 度 ， 
机 械 臂 有 3 个 自由 度 。 对 于 本 文 的 机 器 人 ， 由 于 运动 具有 宛 余 度 ， 该 机 器 人 的 
运动 学 正解 唯一 ， 运 动 学 逆 解 不 唯一 。 因 此 需要 添加 约束 或 优化 指标 ， 本 文 应 
FAD - 互 法 建立 机 器 人 的 运动 学 模型 4 ,481 Е 5 — TTRRT 研磨 机 器 人 的 动力 
学 系统 中 ， 存 在 着 耦合 关系 和 非 线性 。 常 用 的 动力 学 建 模 方法 包括 拉 格 朗 日 
(Lagrange) 方法 、 牛 顿 - 欧 拉 (Newton – Euler) 方法 、 高 斯 (Gauss) 方法 、 
BU (Kane) DE., Wer x BIE AMY (Aah - 魏 登 保 (Roberson - Wittenburg) 
方法 等 (%]。 其 中 拉 格 朗 日 方法 是 建立 能 量 的 平衡 方程 ,适用 于 复杂 系统 建 
BLAS) ， 因 此 本 文 基于 拉 格 朗 日 方法 建立 机 器 人 的 动力 学 方程 。 


3.1.1 5- TTRRT 机 器 人 运动 学 模型 


由 于 研磨 工具 末端 与 第 五 关节 固 联 ， 从 5 — TTRRT AES Uds ALS XE f rh ts 
到 人 研磨 工具 末端 建立 的 运动 学 分 析 系 统 如 图 3-1 所 示 ，5 — TTRRT 研磨 机 器 人 
各 杆 件 的 参数 见 表 3-1。 























а) 参考 坐标 系 


3-1 5- TTRRT 研磨 机 器 人 运动 学 模型 
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Ax, 


Axo 








b) 简化 坐标 系 
图 3-1 5-TTRRT 研磨 机 器 人 运动 学 模型 ( 续 ) 


表 3-1 5- TTRRT 研磨 机 器 人 各 杆 件 的 参数 









































连 杆 序号 i Ө, Ө, 0 L L 1, 变量 范围 
1 0 0 0 0 d, -110 - 110 
2 -90 0 0 -50 d, 0 -80 -10 
3 0 0 6, -222 0 0 -20 «20 
4 90 0 0, - 110 0 0 -20 «20 
5 0 90 0 ds 0 47 -130 ~115 
iE, 在 本 文 的 移动 平台 上 层 中 心 处 ， 设 置 连 杆 坐标 系 的 原点 坐标 〈 坐 标 系 00 ) ， 设 坐标 系 0 与 坐标 





























系 1 的 位 置 重合 。 
表 3-1 中 ， 前 一 坐标 系 与 后 一 坐标 系 在 x*、y、z 轴 的 法 向 平面 上 坐标 系 的 
夹 角 分 别 用 0, Ө,, Ө, 表示 ; 前 一 坐标 系 与 后 一 坐标 系 在 x*、y、z 轴 方 向 基于 
坐标 系 原点 的 平移 距离 分 别 用 1, 1. L 表示 ， 其 中 转角 顺 时 针 方向 为 负 ， 首 
时 针 方 向 为 正 ; di, do, 03, 04, ds 为 关节 变量 ,初始 值 都 为 零 ， 具 体 计算 公 
式 见 附录 。 


3.1.2 5- TTRRT 机 器 人 动力 学 建 模 
RSA hr ВН Н BE zr PKZ 











L-K-P (3-1) 
式 中 一 一 拉 格 妆 日 算 子 ; 
天 一 一 系统 的 动能 
忆 一 一 系统 的 势能 。 
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拉 格 朗 日 动力 学 方程 基本 表达 式 为 
dí ðL ; 
rita ааа) (3-2) 
X (3-2) 中 9; 一连 杆 上 某 点 位 置 的 坐标 ; 

0 一 一 连 杆 上 某 点 的 速度 ; 

Ff 一 一 第 i 个 关节 受到 的 力 或 者 力 窍 ， 即 机 器 人 动力 学 所 求解 





的 未 知 量 ; 
/一 一 系统 的 动能 和 势能 之 差 
系统 的 动力 学 模型 
Mo(q)q * Co (q,4)q * Go(q) =т (3-3) 


X (3-3) +, A 5 x1 维 关节 位 置 矢量 ,，M(g) 为 5 x5 维 对 称 正 定 惯 量 
Ж, C(q.q)q AS х1 维 向 心力 转 矩 和 哥 氏 力 转 和 矩 拓 量 ，G(g) 为 5x1 维 重 
JIRE, T= (ЛЛ, Ta Ta Tos) 为 模型 的 广义 输入 力矩 。 

经 过 一 系列 推导 和 简化 运算 , 机 器 人 动力 学 方程 最 终 表 达 式 为 


= È Diğ tluit УУ YO; k q +, (3-4) 


式 (3-4) 中 ， 由 于 该 机 器 人 是 五 自 由 度 宙 器 人 ， 取 =5。D; 称 为 关节 1 
和 /7 TREE PEE, KARI iAy 的 加 速度 54; 和 2 将 在 关节 i 或 j 上 分 别 产 生 一 
个 等 于 Diq; BY D g; 的 惯性 力 ; WR i=j, Вр DRKAT i B ACE, AA 
关节 i cg 将 在 关节 i 上 产生 一 个 等 于 Dig; 的 惯性 力 ; Lost 
等 效 转动 惯量 ， 对 于 平 动 关节 ，7,; 为 等 效 质量 ，D, 表 示 关 节 i 处 的 重力 。 


n 


D; - pu p Penka En &^8, 82,78, +", 78,] + [Pd; p] + 


ix'"pxx “jx iz" pzz 








р = max. ig 


[т (4; x?6; +d; x^8;) |} 


(3-5) 
Dj = Ут, г, (3-6) 
р=і 
具体 计算 公式 见 附录 。 
3.1.3 5-ТТЕКТ 机 器 人 运动 学 动力 学 仿真 
假设 研磨 工具 头 的 期 望 轨 迹 为 
x = —rsinwt 
у=0 (3-7) 


z= —г(1 — созо?) 
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Е: r=60mm， 研 磨 工具 头 匀速 运动 ， 仿 真 时 间 设 为 20s， 角 速度 o = 
1?/s, 

应 用 前 述 的 运动 学 与 动力 学 模型 ， 结 合 附录 中 的 计算 过 程 ， 得 到 仿真 结 
如 图 3-2 ~ 图 3-4 所 示 。 





单位 : mm 
图 3-2 工具 头 轨迹 图 



























































速度 /mmy/s) 及 角速度 /Crad/s) 





时 间 /s 
Ui. Us 分 别 是 关节 1 和 5 的 速度 ; %3 是 关节 3 的 角速度 。 


图 3-3 三 个 关节 的 速度 时 间 曲 线 














驱动 力 /kN 
e 
Š 












































时 间 /s 
а) 第 1 移动 关节 的 驱动 力 时 间 曲 线 


图 3-4 各 关节 驱动 力 时 间 曲 线 
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驱动 力矩 Nmm 
`8 



































4 6 8 10 12 14 16 18 20 
时 间 /s 
第 3 转动 关节 的 力矩 时 间 曲 线 


32,5 т T T T T T T T 


c 
— 
R 

















驱动 力作 












































时 间 /s 
с) 第 5 平 动 关节 的 驱动 力 时 间 曲 线 


图 3-4 各 关节 驱动 力 时 间 曲 线 (BE) 














3.2 自由 曲面 的 分 片 规划 


3.2.1 自由 曲面 特征 的 基础 


1. 曲面 法 曲率 的 计算 (9 
设 被 加 工 曲面 为 光滑 曲面 ， 其 参数 表达 式 的 一 般 形 式 为 


S;(u,v) = Уз, v) = Xo u^. 1)0, (5? v v 1)! (3.8) 








式 中 S;(u,v) 第 i 个 曲面 片 的 参数 方程 ， 
Q,—A x4 Med | 个 曲面 片 的 矢量 方 阵 ; 
и, v 变量 , Oxu, vxl; 
/一 一 曲面 片 数 。 


对 于 曲面 上 的 任意 一 点 ， 可 求 得 下 列 3 个 矢量 
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_ 95(и,ь) ‚5, = Blas v) P Sn XS, 


i ðu ~ |5, х5, | 
式 中 5S,，5, 一 一 曲面 沿 u、 Бе S 


$ 








由 法 向 曲率 的 含义 可 得 法 向 曲率 公式 





w' Dw 
= 3-9 
” wlGw ( ) 
I Sa ` Š, Š, 7 Dy 
式 中 бе|} c-| [= | 
Y S, É Š, 5, d S, 821 822 


$,*n Š, n d d 
»-| uu uv || 11 2 

Эа "n Š, "n dz d», 

G. D 分 别 为 曲面 的 第 一 、 第 二 微分 基本 形式 ， 
| S(u,v) | 9 S(u,v) _ O'S(u,v) 
Suu = диди ` Sw = ðuðv ^" S. = додо 
& В = руй + dig - 246), MIX (3-9) 可 以 得 到 曲面 上 任意 一 点 的 
主 曲率 А, з k min 











_В+ + VB? -4|G| D | 
k max = 2|G| =k. (3- 10) 
_B- /B°-4 G|D | 
Каш = 2|G| =k, (3-11) 
2. 自由 曲面 的 区 域 分 类 in 


根据 微分 几何 关于 曲面 区 域 的 定义 可 知 ， 
曲面 的 全 曲率 = RS. ki, ky 为 曲面 的 两 
个 主 曲 率 。 

(1) 椭圆 域 。 当 和 =k。 >0 时， 说 明 
在 该 区 域内 曲面 的 弯曲 朝向 切 平面 的 同一 侧 ， 
如 图 3-5 所 示 ， 这 种 区 域内 的 点 没有 渐 近 方 
向 的 区 域 称 为 椭圆 域 ,球面 上 点 也 都 属于 这 
一 类 情况 。 

(2) 双 曲 域 。 当 天 = 天. ky <O 时 ， 这 种 图 3-5 ЖИШШ 
曲面 上 各 点 的 两 个 主 曲率 的 正 负 号 不 同 的 区 域 称 为 双 曲 域 。 根 据 其 连续 性 ， 必 
存在 主 曲率 , 20 的 两 个 浙 近 方向 ， 在 整个 区 域内 ， 必 存在 将 曲面 分 成 沿 两 渐 
近 方 向 的 两 对 对 顶 角 区 域 的 浙 近 曲线 族 ， 在 每 个 对 顶 角 区 域内 各 自 朝 相同 的 方 
向 弯曲 ， 相 邻 角 区 域内 的 曲面 则 向 异 向 弯曲 ， 形 似 马 鞍 ， 如 图 3-6 所 示 。 
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渐进 方向 m 
主 方向 
3-6 双 曲 面 
(3) ШУК, А-А .k=0 
时 ， 可 知 =k, =0 为 平面 ， 但 一 般 不 
是 平 点 ， 所 以 仅 有 一 个 主 曲率 为 零 ， 
Hl E, 20 MA, 20, PRIX FER DC Bi A DU 
物 域 ， 如 图 3-7 所 示 。 不 失 一 般 性 ， 
ЖЕ, =0, Wk, =0 所 对 应 的 主 方向 是 
渐 近 方向 。 由 欧 拉 公 式 k, = k, sin? 可 
知 ， 除 渐 近 方向 外 ， 抛 物 域内 任意 一 
点 其 他 方向 的 曲面 朝 对 应 的 同一 方向 弯曲 。 
曲面 微分 几何 学 是 刀具 轨迹 规划 算法 的 最 重要 理论 基础 ， 在 机 器 人 研磨 加 
工 过 程 中 ， 刀 具 相 对 于 加 工 曲 面 的 位 置 关系 式 就 是 刀具 与 空间 曲面 的 几何 关系 
表达 方式 。 根 据 以 上 叙述 所 得 各 类 区 域 的 特征 可 知 ， 机 器 人 研磨 加 工 的 刀具 选 
择 和 路 径 规 划 ， 应 依据 曲面 的 这 些 特 征 来 制定 。 


3.2.2 自由 曲面 分 片 规划 的 提出 与 分 片 研磨 方法 


根据 大 型 模具 自由 曲面 在 研磨 精 整 加 工 过 程 中 ， 对 研磨 路 径 与 研磨 工具 的 
姿态 及 接触 作用 力 的 要 求 ， 以 及 解决 研磨 精度 和 效率 问题 的 需要 ， 从 保证 研磨 
工具 按 规划 的 路 径 精 确 运动 ， 同 时 又 必须 保持 研磨 工具 与 待 加 工 曲 面 之 间 特 定 
的 姿态 角 及 稳定 的 接触 作用 力 两 个 方面 研究 实施 的 措施 。 根 据 5 -TTRRT 研磨 
机 器 人 的 工作 特点 ， 首 先 依据 人 工 研磨 大 型 模具 自由 曲面 时 ， 针 对 不 同 区 域 使 
用 不 同类 型 研磨 工具 的 特点 ， 分 析 人 工 研磨 过 程 中 ,不 同 区 域 分 片 规划 的 方 
法 ， 将 影响 曲面 片 划 分 与 选择 的 有 关 因素 ， 作 为 建立 数学 模型 的 约束 条 件 ， 然 
后 根据 计算 机 图 形 信息 构造 的 特点 ， 应 用 相应 的 算法 ， 提 取 大 型 自由 曲面 上 一 
系列 曲面 信息 与 制造 特征 相似 的 区 域 ， 将 这 些 区 域 划 分 为 一 系列 特征 相同 或 相 
似 的 曲面 片 ， 即 可 得 到 不 同 的 曲面 片 族 ; 为 大 幅度 提高 研磨 效率 ， 对 相同 曲面 
片 族 进行 研磨 机 器 人 运动 规划 时 ， 选 择 相 同 的 研磨 工艺 条 件 ， 包 括 研磨 工具 
头 、 研 磨 切 削 参 数 的 选择 等 。 其 次 ， 对 于 每 一 个 曲面 片 进行 加 工时 ， 加 工 路 径 





图 3-7 抛物 面 


















































第 3 章 机 器 人 研磨 自由 曲面 的 运动 规划 37 





的 规划 、 工 具 姿 态 及 不 同 研磨 工具 的 选择 ， 根 据 本 章 的 轨迹 规划 方法 进行 确 
定 。 为 了 保证 研磨 工具 的 姿态 变化 相对 平缓 ， 减 少 工具 头 位 姿 变化 引起 的 冲 
击 。 在 同一 曲面 片 族 内 的 曲面 片 ， 采 用 相同 的 工具 头 和 相近 的 工艺 参数 进行 研 
磨 加 工 。 对 于 不 同 曲面 片 间 的 过 渡 选 择 相应 的 运动 规划 ， 就 可 以 实现 从 单个 曲 
面 片 至 曲面 片 族 再 到 整个 曲面 的 加 工 。 

L. 分 片 算法 0501 

首先 根据 第 2 章 的 曲面 重 构 模 型 得 到 的 数据 ， 提 取 曲 面 分 片 所 需 的 几何 参 
数 和 加 工 参数 ， 对 待 加 工 曲面 计算 具体 的 型 值 点 处 的 曲面 参数 包括 高 斯 曲率 、 
平均 曲率 、 主 曲率 、 法 矢 和 干涉 参数 等 。 对 计算 所 得 的 这 些 参数 值 ， 应 用 基于 
曲率 法 和 中 值 搜索 法 确定 该 曲面 片 边 界 和 曲面 片 的 中 心 位 置 。 由 于 三 维 笛 卡 儿 
空间 得 到 自由 曲面 上 各 点 的 曲率 不 同 ， 根 据 上 述 三 种 曲面 区 域 的 分 类 方法 ， 采 
用 基于 曲率 的 分 片 规划 ， 利 用 式 (3-10) 和 式 (3-11) 进行 自由 曲面 上 每 个 
型 值 点 处 的 主 曲 率 值 求解 ， 可 以 得 到 整个 曲面 的 最 小 曲率 半径 К, 


























1 
R,= 3-12 
P LED pea) 


然后 求 其 邻近 的 型 值 点 处 的 曲率 半径 已 
R, -R;=e Cle] <Q) (3-13) 

式 (3-13) F, 2 的 取 值 受到 多 个 因素 的 影响 。 研 磨 工具 的 类 型 、 研 磨 工 
具 尺 十 的 大 小 、 研 磨 加 工 的 工艺 参数 和 研磨 机 器 人 作业 空间 的 变化 ,决定 了 О 
取 值 的 不 同 。 将 每 次 求 出 的 当前 型 值 点 处 R, 与 前 一 个 进行 比较 ， 若 求 出 的 差 
[А e 满足 给 定 的 公差 0， 则 需要 继续 向 周边 搜索 ， 所 得 的 型 值 点 范围 向 周围 扩 
大 ， 直 到 搜索 到 不 再 符合 式 (3-13) 的 条 件 点 为 止 ， 就 得 到 所 要 获取 曲面 片 
的 边界 点 ， 即 不 符合 条 件 的 点 。 将 得 到 的 每 个 曲面 片 所 有 边界 点 连 成 边界 线 ， 
这 样 就 可 以 得 到 一 个 具体 的 曲面 片 及 其 边界 。 对 于 得 到 的 竺 加工 自由 曲面 上 的 
不 同 曲面 片 ， 根 据 曲面 信息 和 研磨 工艺 信息 相同 或 相似 的 原则 ， 划 分 出 若干 个 
相似 的 曲面 片 族 。 具 体 算法 如 下 : 

自由 曲面 上 的 一 个 参数 曲面 片 设 为 5S,(w,v) ， 其 参数 区 域 为 [a,b] x[c,d]， 
P, ;为 该 曲面 片上 的 型 值 点 ， 在 数组 C 中 记录 上 述 信息 。 

步骤 一 : 求解 自由 曲面 面 上 最 小 曲率 半径 处 К, 对 应 的 型 值 点 Ps， 给 定 
正 数 0 的 值 ， 计 算 最 小 曲率 半径 Ro 

FR: гуп, MHE 已 ,处 的 曲率 半径 R,， 否 则 执行 步骤 四 。 

步骤 三 : 判断 |s| <Q 条 件 是 否 成 立 ， 若 成 立 则 记录 Р, ;处 的 数组 А, j= 
J+1， 执 行 步骤 二 ; 否则 将 P. ;记录 到 数组 了，j =j +1， 执 行 步骤 一 。 

PRO. 若 i<m， 则 取 i=i+1， 执 行 步骤 二 ; 否则 计算 结 
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步骤 五 : HAHA, BA C 中 的 数据 进行 整理 ， 数 组 4 中 的 数据 记录 了 
一 组 特征 相似 的 型 值 点 ， 据 此 可 得 到 一 个 曲面 片 ， 数 组 B 中 记录 的 i j 极 值 
点 是 曲面 片 的 边界 点 。 应 用 中 值 搜索 法 对 同时 满足 两 个 曲面 片 的 点 进行 判断 ， 
然后 将 4、B 和 C 中 的 数据 继续 代入 步骤 一 的 条 件 中 重复 进行 判断 ， 直 到 遍历 
整个 待 加 工 自由 曲面 。 

图 3-8 为 自由 曲面 分 片 流程 图 。 





















记录 到 
数组 C 










给 定 NURBS 曲 面 的 集合 信息 


求 最 小 曲率 半径 R 








给 定 Rr 处 的 型 值 点 PP 、Py;， 设 
定 的 约束 条 件 为 Q 





























图 3-8 自由 曲面 分 片 流程 图 
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2. 曲面 边界 定义 

基于 上 述 的 基于 曲率 的 自由 曲面 分 片 算法 ， 以 及 判定 边界 点 的 中 值 搜索 
法 ,结合 第 2 章 得 到 的 曲面 模型 的 重 构 数据 ， 完 成 待 加 工 自由 曲面 的 分 片 和 曲 
面 边界 的 确定 ; 根据 所 有 在 同一 个 曲面 片上 的 网 格 点 有 相似 的 特征 ， 对 构成 曲 
面 片 族 的 网 格 点 ， 依 据 每 个 点 特征 信息 定义 的 相似 性 原理 ， 即 每 个 昌 面 片 中 心 
点 的 特征 可 代表 同一 个 曲面 片上 所 有 网 格 点 的 特征 ， 则 曲面 片 中 心 点 就 可 代表 
这 些 点 的 特征 。 而 由 同一 个 曲面 片上 各 个 点 都 具有 相似 的 几何 性 质 ， 就 可 得 到 
曲面 片 数 目 、 曲 面 片 边界 和 子 曲 面 片 的 中 心 位 置 。 
3.2.3 ”自由 曲面 的 加 工 路 径 规 划 

1. 研磨 工具 加 工 方向 上 的 刀 触 点 获取 '!”] 

根据 给 定 的 精度 (MRE) 要 求 ， 如 图 3-9。 采 用 上 一 章 的 方法 得 到 的 
待 加 工 曲面 模型 ， 对 提取 并 经 3 次 均匀 B 样 条 处 理 后 的 激光 线 上 的 数据 点 ， 
进行 匈 余 处理 ， 取 得 对 应 于 精度 要 求 的 刀 触 点 数据 。 假 设 P| Pa, Pa, Р, 为 处 
































理 后 激光 线 上 顺序 相 邻 的 4 个 数据 点 ， 如 图 3- 10 ， 刀 触 点 数据 生成 过 程 如 下 : 
Р» 
Р; 
М, 
Р, P4 
图 3-9 加 工 步 长 示意 图 3-10 ЛА 


步 又 一 : 保留 起 始 测 点 Pi。 

步 又 二 : ЖЕР, 点 与 其 后 相隔 的 P 点 。 

步骤 三 : 判断 连 线 中 间 的 P, 点 误差 球 (以 该 点 为 球 心 ， 做 出 半径 为 给 定 
允许 弦 高 误差 的 误差 球 ) 与 PP, EAE AAAS , 

步骤 四 : 若 中 间 点 Р, 误差 球 与 连 线 P P, 相交， 说 明 中 间 点 P, 在 允许 弦 
高 误差 范围 内 ， 则 去 除 P, 点 ; Р, 再 与 下 一 点 (P, 点 ) 连 线 ,重复 步骤 三 的 
过 程 。 

步骤 五 : 若 连 线 PP, 与 中 间 点 P, 的 误差 球 不 相交 ， 则 该 P, HOS, T 
以 保留 ， 将 P, 作为 起 始点 ， 重 复 步骤 二 至 步骤 五 过 程 ， 直 到 该 曲线 上 最 后 一 
个 点 (保留 最 后 一 个 点 )。 

步 又 六 : 对 应 该 曲线 研磨 工具 加 工 方向 上 的 刀 触 点 轨迹 由 上 述 所 有 保留 点 
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构成 。 
步 又 七 : 提取 每 个 刀 触 点 处 的 局 部 几何 信息 ， 可 转化 得 到 相对 应 的 刀 位 点 
轨迹 。 


图 3- 10 说 明了 如 何 判断 一 个 中 间 点 的 误差 球 是 否 与 相应 的 连 线 相交 ， 假 
BP, 点 到 忆 点 的 矢量 用 MM 表示 ,Pi 点 到 P. 点 矢量 用 N 表示 , Kit M. N 
的 值 为 








М =(a,,b,,¢,), N= (a5, b, ,05) 
判断 连 线 P,P, 是 否 与 P, 点 的 误差 球 相 交 ， 就 是 比较 误差 球 的 球 心 (Р, 
点 ) 到 相应 连 线 的 弱 高 d 是 否 大 于 误差 球 的 半径 +。 若 误差 球 与 相应 连 线 不 相 
Ж, M d>r; 反之 ,误差 球 与 相应 连 线 相交 ， 则 d<r; dg =r 时 ,可 根据 需要 
灵活 处 理 。 根 据 微分 几何 与 线性 代数 的 知识 可 知 ， 中 间 点 P, 到 连 线 P.P, 的 
距离 d 为 





IMxN| 
d= 
IN | 


代入 M 和 的 分 量 值 ， 则 d 可 表示 为 
(b, хе =b, xa) + (с Xa, —6; xa)? + (a, xb, -a xb)? 


2 2 2 
fas +b5 + c; 


(3-14) 





d= 





(3-15) 
2. 研磨 行距 的 确定 
如 图 3-11 所 示 ， 应 使 相 邻 的 两 条 加 工 路 径 所 形成 的 残余 高 度 ó 控制 在 允 
许 范围 内 ， 对 磨 削 行 间距 规划 分 平面 、 凸 曲面 和 四 曲面 三 种 情况 ， 设 相 邻 的 刀 
触 点 为 Pl 、P,， 曲 线 P P, 之 间 的 平均 曲率 半径 为 p， 可 以 看 出 行 间距 的 大 小 
AL 与 研磨 工具 头 的 半径 >、 研磨 加 工 允 许 的 最 大 残余 高 度 5、 被 加 工 曲面 的 几 
何 特征 有 关 。 




















平面 :AL=2 /275 -8 (3-16) 

Ar P (4m +4r° -28p - 6^) (28р & 8^) Е 
— (p +r) (p +8) ns 
MEH. AL =? (4rp - 4r? -26p +87 ) (28p — 8^) (3-18) 





(p-r)(p-ó) 
式 中 ”Al 一 一 行 间距 ，; 
r 一 一 研磨 工具 头 半径 ; 
6 一 一 残余 高 度 ; 
p 一 一 相 邻 刀 触 点 曲线 P, P, 之 间 的 平均 曲率 半径 。 
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а) 平面 研 麻 b) 凸 面 研磨 c) 四 面 研磨 
图 3-11 研磨 行 间 距 示 意图 


3.3 5 一 TTRRT 研磨 机 器 人 的 位 姿 与 运动 规划 


3.3.1 移动 平台 姿态 控制 策略 


5 -TIRRT 机 器 人 的 移动 平台 以 直角 坐标 方式 运动 时 ， 运 动 到 每 一 个 加 工 
区 域 后 ,在 蕊 向 机 构 端 部 和 了 向 机 构 的 端 部 四 个 电动 推 杆 伸 出 不 同 的 行程 ， 
电动 推 杆 下 部 连接 的 吸盘 吸附 在 被 加 工 自由 曲面 的 表面 ， 由 于 曲面 的 表面 轮 廊 
导致 四 个 电动 推 杆 伸 出 的 行程 不 同 及 存在 偏差 等 原因 ， 可 能 造成 机 器 人 移动 平 
台 倾 斜 ， 因 此 需要 姿态 校正 过 程 。 进 行 移动 平台 姿态 调整 的 目的 是 使 移动 平台 
平行 于 水 平面 ， 本 文采 用 倾斜 计 反 馈 校 正 的 机 器 人 平台 姿态 补偿 策略 。 

机 器 人 移动 平台 的 姿态 控制 构成 如 图 3- 12 所 示 ， 为 方便 起 见 ， 将 移动 平 
台 的 * 轴 正 向 、x 轴 负 向 、y 轴 正 向 、y 轴 负 向 的 四 个 电动 推 杆 运动 分 量 设 为 
Sa. Sa, Sa, Sa, TEI x 向 与 y 向 反馈 的 双 轴 倾角 传感器 安装 在 机 吉 人 移 
动 平台 的 最 上 部 ， 其 性 能 指标 见 表 3-2。 






































测 x 方 向 倾斜 计 模拟 量 输入 






测 y 方 向 倾斜 计 模拟 量 输入 














图 3-12 机 器 人 移动 平台 的 姿态 控制 构成 
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表 3-2 倾斜 计 的 主要 参数 


























测量 范围 精度 再 现 性 | 交叉 轴 方 向 误差 | ”温度 范围 “| ”温度 特性 
+45° 0.01? +0. 05° <1% -40? ~60° 0. 008% 





安装 在 机 器 人 本 体 上 的 姿态 传感器 (倾斜 计 ) ， 可 以 实时 地 测 出 机 器 人 移 
动 平台 所 在 平面 与 水 平面 之 间 的 夹 角 ， 通 过 该 夹 角 ， 可 以 知道 移动 平台 姿态 变 
化 的 程度 。 当 倾斜 计 输 出 的 电压 信号 经 A/D 转换 成 数字 量 ， 送 入 控制 计算 机 。 
通过 给 不 同 的 电动 推 杆 施 加 相应 的 开关 量 ， 即 可 实现 对 移动 平台 的 姿态 调节 ， 
该 环节 可 通过 控制 系统 软件 实现 : 

IF|g,|<6H ^ THEN er=0 
ELSE ф,=е, 

要 求 5 - TTRRT 机 器 人 的 移动 平台 能 够 在 自由 曲面 上 保持 水 平 的 姿态 ， 即 

Ф„=0, TÆ 











Ap =P, -P= -Ф, (3-19) 

式 (3-19) 中 ，9pr 一 一 倾斜 计 读 数 ， 单 位 为 〈") ; 9p。 一 一 要 求 姿态 角 ， 

单位 为 "， 对 Ao 进行 运算 得 到 调整 量 ， 对 应 5, So. 54. Sa, ПАН 4 

电动 推 杆 的 位 移 差 值 ， 然 后 输入 电动 推 杆 位 置 控制 单元 ， 就 构成 了 移动 平台 姿 
态 闭环 控制 系统 。 


3.3.2 ”研磨 工具 位 姿 在 机 器 人 运动 空间 中 的 表达 


一 般 情况 下 ， 研 磨 工 具 与 工件 表面 之 间 的 关系 ， 表 示 为 空间 线段 与 空间 曲 
面 之 间 的 关系 (参见 图 3-13) 0591, 

















zi Po(Xo, уо, 20) 











研磨 工具 : 
X— Xo y -Yo 2-20 
т п р 
工件 曲面 : 
F(x, y, z)-0 








ы 
\ 
=y 


图 3-13 ”研磨 工具 与 工件 表面 的 空间 位 姿 关 系 1153] 
假设 在 工件 坐标 系 中 工件 表面 的 方程 为 
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F(x,y,z) =0 (3-20) 
研磨 工具 在 工件 坐标 系 中 的 方程 为 


kx y-yo 2-20 





(3-21) 


т п р 
AP, xo. yo. zo 为 研磨 工具 接触 点 Po (xo, yo ,zo ) 的 坐标 ， 而 (m,n,p)" 为 研 
磨 工具 的 位 姿 撩 量 。 则 Po 点 曲面 的 切面 方程 为 

A(x -%)) * B(y-yg) +C(z-z0) +D=0 (3-22) 
其 中 : 


OF (x,y,z) 
pude Mu 


Ox P =Po? =Po ; 


gaara jp Ce 
ду 


De-A*:g*uy-B*g*yo-C* Et dg 
设 研磨 工具 与 自由 曲面 外 表面 上 某 点 切面 的 夹 角 为 和 ， 则 从 -$-B (B 


为 研磨 姿态 角 见 图 3- 14) ， 得 到 
lA*g*:m*B*g*n*C*g*p 
AT EE ups mn p 

对 于 给 定 的 待 加 工 自由 曲面 R(x,y,z) =0， 由 第 2 章 得 到 曲面 模型 的 几何 
言 息 ， 可 以 计算 得 出 式 (3-23) 中 所 和 需 的 4、B、C， 所 以 研磨 工具 与 工件 曲 
面 在 工件 坐标 空间 的 位 姿 撩 量 (m,n,p)? 可 以 由 研磨 工具 的 姿态 角 B 确定 。 在 
自由 曲面 的 研磨 加 工 过程 中 ， 姿 态 角 В 保持 恒定 ， 才 能 保持 研磨 压力 恒定 ， 因 
为 自由 曲面 上 的 型 值 点 切 平 面 与 其 曲率 半径 相互 关联 ， 所 以 自由 曲面 自动 研磨 
时 ， 其 研磨 工具 位 姿 会 随 着 曲面 上 型 值 点 的 曲率 半径 变化 而 实时 变化 。 

建立 如 图 3-14 所 示 的 工具 头 刀 触 点 与 刀 位 点 关系 图 ， 对 于 自由 曲面 研磨 
刀 位 点 ( 工 点 ) 的 轨迹 ， 可 由 刀 触 点 (C 点 ) 轨迹 数据 通过 研磨 工具 头 主轴 
与 刀 触 点 公法 线 的 研磨 姿态 角 B 计算 得到。 

C 点 为 工件 坐标 系 中 的 点 , L SA ONHU Л АК АО, ETE TBAB 
标 到 机 器 人 坐标 的 转换 , LS Р(х,у) EHI СЩ P (х, ,у,,2,) DE x 轴 旋 转 研 





sin A = 


(3-23) 











磨 姿 态 角 В 得 到 。 B 

X 1 0 0 0 Ne 

yi _ 0 кеп -sinf 0 : Ve (3-24) 

£j 0 sng cosB 0 2, 

1 0 0 0 1 1 E " 





3-14 JPL Jf AAI 
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化 简 可 得 
XI =X. 
Yı = y,cos В – z,sin B (3-25) 


д = y,sin В +z cos В 


由 此 可 解决 工件 坐标 系 到 机 器 人 坐标 系 的 转换 。 
3.3.3 ”自由 曲面 研磨 精 加 工 的 行 切 法 轨迹 规划 


以 对 工件 实体 在 x 方向 上 进行 行 切 分 层 剖 切 为 例 ， 如 图 3-15a， 可 得 分 层 
剖面 与 工件 外 表面 相交 的 空间 曲线 。 在 工件 坐标 系 Oxyyiz, 中 ， 用 一 个 与 
у 012 平面 平行 的 平面 ABCD 与 工件 相交 截 得 的 相交 面 ， 如 图 中 点 画 线 内 的 部 
BY, 该 相交 区 域 的 外 围 边 界 曲 线 既 是 需要 得 到 的 行 切 法 的 空间 轨迹 。 行 切 法 都 
是 在 平行 于 у О, 的 不 同 平面 内 进行 轨迹 规划 ， 人 研磨 工具 所 走 过 的 轨迹 都 在 
这 些 平行 的 平面 内 ， 在 每 个 平面 内 需要 保证 研磨 工具 运动 时 位 姿 角 BB 位 姿 正 
确 ， 研 磨 工 具 的 位 姿 如 岁 3-15b， 从 图 中 可 看 出 ， 研 磨 工 具 头 治 着 行 切 轨迹 工 
作 时 ， 由 于 空间 轨迹 的 曲率 变化 ， 为 了 保持 要 求 的 位 姿 角 B， 工 具 头 的 刀 触 点 
应 能 随 着 曲率 变化 而 调整 ， 每 相 邻 两 个 平面 的 距离 就 是 每 次 研磨 行距 。 

















Xy OI [2 


а) 工件 模 截 面 位 置 及 方向 b) 研磨 工具 模 截 面 位 置 及 方向 





图 3-15 自由 曲面 研磨 精 加 工 的 行 切 法 轨迹 规划 原理 











由 图 3-15b 所 示 ， 在 每 个 平行 于 y101z1 平面 的 平面 ABCD 内 ,人 研磨 工具 
治 着 行 切 法 的 轨迹 工作 时 ， 研 磨 工 具 头 轮廓 在 每 一 个 刀 触 点 位 置 与 相交 截面 的 
工件 表面 曲线 相 外 切 。 不 论 研磨 工具 处 于 被 加 工 曲面 的 何 种 方位 ， 其 在 运动 中 
应 该 始终 与 平面 y,01z1 平行 。 
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3.3.4 自由 曲面 研磨 精 加 工 的 环 切 法 轨迹 规划 


采用 图 3- 16b 所 示 的 环 切 法 原理 ， 在 图 中 z 方 向 上 ， 通 过 一 系列 与 x101y1 
平面 平行 的 平面 对 工件 实体 进行 分 层 ， 即 可 得 工件 轮廓 表面 与 分 层面 相交 的 空 
间 曲 线 。 该 空间 曲线 是 研磨 工具 加 工 的 期 望 轨迹 ， 如 图 3- 16a 所 示 ， 在 工件 坐 
标 系 Ox. yz, 中 ， 图 中 阴影 部 分 所 示 的 相交 面 是 被 某 一 与 x,0,y, 平面 平行 的 
平面 4BCD 与 工件 相交 所 截 到 的 ， 相 交 区 域 的 工件 外 于 边界 曲线 ， 既 是 环 切 法 
需要 的 空间 轨迹 。 与 行 切 法 加 工 相 同 ， 在 进行 环 切 法 轨迹 规划 时 ， 既 要 规划 出 
研磨 工具 头 所 走 过 的 轨迹 ， 同 时 还 需要 保证 工具 头 运动 位 姿 角 B 正确， 工具 的 
位 姿 如 图 3-16b， 研 磨 工具 头 的 刀 触 点 分 布 在 一 系列 等 高 线 上 ， 若 同一 等 高 线 
上 各 点 的 曲率 相同 ， 则 工具 头 的 位 姿 角 B 可 保持 不 变 。 和 否则 ， 为 了 保持 要 求 的 
工具 头 位 姿 角 B， 研 磨 工 具 头 的 刀 触 点 应 随 着 同一 等 高 线 上 各 点 的 曲率 变化 而 
调整 ， 相 邻 两 层 等 高 线 的 距离 就 是 每 次 研磨 抬 刀 或 进 刀 的 距离 。 




















“FO; Л 


а) 工件 模 截 面 位 置 及 方向 b) 研磨 工具 横 截 面 位 置 及 方向 











图 3-16 自由 曲面 研磨 加 工 环 切 法 轨迹 规划 原理 图 





3.3.5 行 切 法 轨迹 规划 与 环 切 法 轨迹 规划 的 比较 


应 用 本 文 的 5 — TTRRT 机 器 人 进行 自由 曲面 的 研磨 精 整 加 工时 ， 首 先 ， 通 
过 移动 平台 的 运动 ， 使 机 器 人 到 达 一 个 指定 的 加 工区 域 ， 然 后 通过 机 械 臂 带动 
研磨 工具 完成 该 区 域 的 加 工 。 由 于 研磨 工具 位 姿 的 实时 变化 ， 应 用 上 述 的 两 种 
轨迹 规划 方法 对 空间 曲面 加 工时 ， 行 切 法 可 采用 往复 走 刀 的 方式 实现 对 待 加 工 
区 域 的 加 工 ， 在 不 同 的 剖面 内 ， 根 据 研磨 工具 的 路 径 规划 ， 研 磨 工具 的 运动 由 
一 个 直线 运动 、 一 个 转动 和 一 个 摆动 的 三 维 空间 动作 组 合 完成 ， 在 每 次 的 二 维 
平面 中 ， 走 刀 轨 迹 由 一 个 直线 运动 和 一 个 转动 的 组 合 完成 ， 摆 动机 构 实现 的 是 
保证 切削 行距 的 情况 下 ， 完 成 下 一 次 走 刀 ; 环 切 法 采用 的 分 层 加 工 〈 既 等 高 
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ШТ.) 的 方式 ， 在 不 同 的 等 高 面 内 ， 根 据 研 磨 工具 的 路 径 规 划 ， 研 磨 工具 的 
运动 由 一 个 直线 运动 、 一 个 转动 和 一 个 摆动 的 三 维 空间 动作 组 合 来 实现 ， 在 每 
一 等 高 面 内 ， 走 刀 轨 迹 由 一 个 直线 运动 、 一 个 转动 与 一 个 摆动 的 三 维 动作 组 合 
完成 ，z 方向 保证 的 是 在 相 邻 两 层 间 抬 刀 或 降 刀 ， 对 于 本 文 的 5 — TTRRT В 
人 来 说 ， 对 于 每 一 次 的 走 刀 过 程 ， 行 切 法 路 径 规划 所 要 求 的 运动 关节 组 合 少 于 
环 切 法 的 组 合 要 求 。 因 此 ， 行 切 法 具有 便于 控制 、 干 涉 少 和 效率 高 的 特点 。 对 
自由 曲面 研磨 精 整 加 工 ， 应 用 5 - TTRRT 机 器 人 的 行 切 法 路 径 规划 整体 效果 要 
优 于 环 切 法 路 径 规 划 ， 本 文 主要 针对 行 切 法 的 加 工 方式 进行 相关 的 研究 。 





Жал BL ae AERE FI tH dl Iñi i 
轨迹 跟 踊 控制 策略 


早期 大 多 数 工业 机 器 人 在 控制 算法 上 ， 都 采用 简单 的 PD (或 PID) 控制 
算法 。 在 不 需要 了 解 机 器 人 动力 学 模型 的 前 提 下 ， 针 对 速度 与 精度 要 求 不 高 的 
场合 ， 不 基于 模型 的 控制 方法 ， 即 PD (或 Рр) 控制 算法 具有 良好 重 棒 性 。 
然而 对 于 快速 、 高 精度 轨迹 跟踪 控制 的 情况 ， 增 大 控制 增益 的 办 法 无 法 使 PD 
(A PID) 控制 非 线 性 系统 全 局 收 化 。 为 获得 更 好 的 动态 跟踪 性 能 ， 需 要 建立 
基于 模型 的 控制 器 。 机 器 人 复杂 的 、 强 耦合 的 非 线 性 动力 学 模型 ， 在 高 品质 的 
机 器 人 跟踪 控制 时 应 该 全 面 予 以 考虑 。 为 了 实现 闭环 误差 系统 的 稳定 和 抑制 干 
扰 ， 即 从 轨迹 跟踪 误差 尽快 趋 于 零 与 尽 可 能 地 减 小 干扰 信号 对 跟踪 精度 的 影响 
两 个 方面 进行 处 理 。 基 于 机 器 人 模型 的 轨迹 跟踪 控制 方法 主要 有 前 馈 控 制 方 
法 、 计 算 力 矩 控制 方法 、 自 适应 控制 方法 、 变 结构 控制 方法 等 。 由 于 在 实际 系 
统 中 ， 很 难得 到 机 器 人 动态 的 精确 数学 模型 ， 各 种 基于 标 称 系统 和 不 确定 性 的 
描述 参数 的 数学 模型 来 设计 控制 器 的 方法 ， 被 越 来 越 多 的 学 者 研究 。 本 章 利用 
了 上 一 章 的 动力 学 模型 ， 在 考虑 机 器 人 系统 的 不 确定 性 前 提 下 ， 建 立 相 应 的 完 
整 动力 学 模型 ， 为 实现 本 文 $ -TIRRT 研磨 机 器 人 的 轨迹 跟踪 控制 ， 通 过 引入 
前 馈 项 与 滑 模 交 结 构 补 偿 ， 基 于 计算 力矩 法 进行 了 PD + 前 馈 补 偿 滑 模 变 结 构 


控制 器 设计 。 
4.1 滑 模 变 结构 基本 理论 


4.1.1 滑 模 变 结构 控制 的 基本 原理 


滑 模 变 结构 控制 的 基本 原理 0541 ， 当 系统 状态 点 穿越 状态 空间 的 不 同 
域 , 达到 不 连续 曲面 (滑动 超 平面 ) 时 ， 控 制 系统 结构 随时 间 变 化 的 开关 特 
性 使 反馈 控制 的 结构 按照 一 定 逻 辑 切 换 变化 ， 从 而 使 得 系统 的 状态 轨 线 能 够 到 
达 期 望 的 滑动 超 平面 ， 并 且 治 着 这 个 超 平面 收敛 到 原点 ， 这 个 超 平面 就 是 滑 模 
面 。 系 统 的 动态 特性 完全 由 所 设计 的 滑动 模 态 决定 ， 设 计 适当 滑 模 参数 和 控制 
律 ， 系 统 的 状态 轨 线 就 能 达到 期 望 的 平衡 点 。 

一 般 形式 的 系统 模型 





> 
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x=f(x,u,t) (4-1) 
X (4-1) 中 ， 状 态 变 量 : xeR", ie R; 控制 输入 : ueR", 
定义 切换 函数 *(x) 
s(x) =X] +6555 t +С, 4X, 4 TX, (4-2) 
这 里 cu 、c,、…、c_1 是 确定 的 系数 ， 在 n 维 向 量 空间 里 , 式 (4-2) 确 
定 了 一 个 s(x) =0 的 超 平面 。 
对 系统 表达 式 (4-1) 的 请 模 变 结构 控制 的 问题 是 求解 系统 的 非 连续 控制 
函数 





u*(x) (s(x) >0) 
u(x) = 4-3 
s bu (s(x) «0) ) 
选择 u* (х) Жит (x) 使 得 
lim s(x) <0 | 
[ere MSS «0 (4-4) 


lim s(x) >O 
s(a)0- 

МАЗ Е ies 5) BL БЕН], ЖОК AR Be IS Bh 25 UE [п] F T8 BE TE РА 
s(x) =0, ASS URBE ER PRCT AE, Tim. 3(%) =0。 式 (4-4) 称 为 滑 模 
面 函 数 s(x) 的 可 达 性 条 件 ; “4 s(x) =0 时 ， 系 统 状 态 到 达 滑 动 超 平面 ， 系 统 
沿 着 此 超 平 面 的 运动 被 称 为 “滑动 模 态 ”或 简称 “ 滑 模 ”; 不 连续 控制 函数 式 
(4-3) 就 是 “ 滑 模 变 结构 控制 ”函数 。 

依据 上 述 定 义 ， 系 统 状态 在 到 达 滑 模 面 后 ， 即 滑动 模 态 下 有 

CX,  €9X5 t o 0 (4-5) 


式 (4-5) 为 系统 状态 的 线性 约束 。 
4.1.2. 滑 模 变 结 构 控 制 的 特点 


在 滑 模 变 结构 控制 过 程 中 ， 系 统 的 结构 根据 系统 当时 的 偏差 及 其 各 阶 导 数 
值 ， 按 设 定 规律 以 上 下 运动 的 方式 做 相应 的 改变 ， 滑 模 变 结构 控制 是 处 理 非 线 
性 控制 系统 的 鲁 棒 控 制 方法 。 变 结构 控制 的 基本 思想 是 首先 把 系统 的 状态 在 有 
限时 间 内 驱使 到 滑 模 面 ， 然 后 沿 此 滑 模 面 滑 动 到 原点 ， 这 种 具有 滑动 模 运 动 的 
控制 也 称 为 滑 模 控制 ( Sliding - mode Control，SMC )。 其 特点 可 归 
纳 为 L154-156] А 

1) 该 控制 方法 具有 不 变性 ， 使 系统 的 运动 与 系统 的 建 模 误 差 和 外 界 干扰 
完全 无 关 。 根 据 变化 范围 ， 可 实现 系统 的 精确 控制 。 

2) 系统 在 滑动 过 程 中 ， 系 统 的 运动 仅 与 滑 模 面 的 参数 有 关 ， 上 有 具有 很 强 的 
鲁 棒 性 。 
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3) 系统 具有 控制 算法 简单 、 易 于 实现 的 特点 ， 适 合 于 机 器 人 控制 。 
4) 系统 不 连续 的 开关 特性 引起 抖 振 (或 称 为 抖动 、 颤 振 等 ) 的 缺点 ， 影 
响 了 其 在 实际 系统 中 的 广泛 应 用 。 


4.2 PD + 前 馈 型 滑 模 变 结 构 补 偿 控 制 


4.2.1 问题 描述 


在 实际 机 器 人 系统 中 ， 由 于 存在 着 建立 被 控 对 象 数学 模型 局 限 性 问题 以 及 
实际 过 程 自 身 参数 摄 动 现象 ， 因 此 ， 机 器 人 系统 中 不 可 避免 地 存在 着 各 种 形式 
的 不 确定 性 501 。 

1) 参数 不 确定 性 : 一 般 不 改变 系统 的 结构 及 阶 次 ， 如 负载 、 连 杆 质量 及 
连 杆 几何 参数 。 

2) 非 参 数 不 确定 性 : 低频 未 建 模 动力 学 特性 ， 包 括 各 关节 的 摩擦 、 关 节 
柔性 等 。 建 模 时 忽略 的 高 频 特性 ， 包 括 驱 动 器 动力 学 特性 、 结 构 共 振 模 式 等 ; 
作业 环境 干扰 、 了 驱动器 饱和 问题 ， 测 量 误差 、 舍 人 误差 及 采样 延迟 等 。 

因此 ， 很 难得 到 式 (3-3) 表示 的 精确 动力 学 模型 。 控 制 系 统 无 法 通过 常 
规 的 线性 控制 方法 得 到 理想 的 效果 ， 对 机 絮 人 系统 的 分 析 和 研究 ， 必 须 按 照 非 
线性 系统 理论 人 处理。 为 提高 机 器 人 的 工作 性 能 与 实现 系统 的 鲁 棱 性 ， 上 述 不 确 
定性 的 影响 因素 ， 在 设计 实际 机 器 人 动态 控制 系统 时 ， 必 须 加 以 考虑 。 结 合 上 
一 章 的 动力 学 模型 ， 在 充分 考虑 这 些 不 确定 性 因素 和 机 器 人 的 受 限 运动 情况 
下 ， 对 于 本 文 的 机 器 人 系统 可 得 到 如 下 完整 动力 学 模型 

M(q)q*C(q.q)d *G(q) +/(q,q,q,t) =т (4-6) 

式 (4-6) 中 ,考虑 了 机 器 人 的 不 确定 因素 后 ，M(9)9、C(9q,0)9、G(9) 
在 物理 意义 上 与 式 (3-3) 是 一 样 的 ,但 是 此 时 的 M(g)i、C(g,9)9、G(g) 是 
时 变量 ; f(g ,4,3,t) 表示 外 界 扰 动量 (包含 非 参数 不 确定 性 、 模 型 不 确定 性 )， 
7 为 作用 在 关节 上 的 5 x 1 维 力 / 转 和 矩 矢 量 。 

本 文中 将 式 (3-3) 称 为 机 器 人 的 标 称 系统 模型 ， 式 (4-6) 称 为 机 器 人 
的 实际 系统 模型 。 

关节 速度 和 笛 卡 儿 空 间 速度 的 关系 可 以 表示 为 

Х=],(4)4 (4-7) 

根据 表示 操作 顺 末 端点 实际 位 移 和 关节 实际 位 移 关 系 的 雅克 比 和 矩阵 J, 

(q), ， 笛 卡 儿 加 速度 项 可 写 为 
Х=],(4)@ +J (q) (4-8) 
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关节 空间 机 器 人 的 运动 方程 在 笛 卡 儿 空 间 坐标 系 中 表示 为 
q-(54) X (4-9) 
g=(J,) (X - 4) (4-10) 
在 雅克 比 和 矩阵 J (q) 的 非 奇 异 的 位 置 ， 根 据 关 节 空 间 与 操作 空间 的 速度 、 
加 速度 关系 ， 由 式 (4-6), xX (4-9) 及 式 (4-10) 得 到 运动 受 限 机 械 手 在 笛 
卡 儿 空间 的 动力 学 方程 为 
М*Х+С*Х+С* =F-F, (4-11) 
式 中 M МЭН, M* =JIMJ;'; 
С° =(J1 CI, - (ЈЕ) MTT 一 一 离 心力 / 哥 式 力 和 矩阵 ; 
G'- (П) G— EJ; 
F.= (J) -7 一 一 外 界 对 机 器 人 的 作用 力 向 量 。 
本 文中 采用 基于 计算 力矩 的 方法 进行 位 置 控制 ， 其 规则 为 
F=M*U+C*X+G* +F, (4-12) 
О=Х, +К,(Х-Х) +K,(K,, - X) (4-13) 
本 文 仿真 所 应 用 的 主要 机 器 人 运动 学 、 动 力学 计算 公式 代码 和 参数 见 第 3 
章 与 附录 。 


4.2.2. PD + 前 馈 补偿 滑 模 控制 器 设计 


基于 系统 在 操作 点 线性 化 并 设计 相应 的 线性 控制 器 是 一 种 常用 的 求解 非 线 
性 控制 的 方法 ， 控 制 器 可 以 保证 系统 是 局 部 稳定 的 ， 并 决定 了 整个 系统 的 稳定 
性 。 如 在 设计 中 引入 速度 滤波 函数 ， 选 用 合适 的 李 亚 普 诺 夫 也 数 可 以 证 明 线 性 
控制 器 是 全 局 稳定 的 [01 。 
机 器 人 的 比例 加 微分 (PD) 控制 规则 就 是 基于 这 样 的 设计 思想 ， 最 简单 
的 PD 控制 规则 具有 如 下 形式 : 
T= -K,é-K,e (4-14) 
AF, K, AK, 为 正定 矩阵 ，e =94 -qu。 参考 文献 【150] 中 分 析 了 基于 PD 的 
3 种 常用 机 器 人 轨迹 跟踪 算法 的 控制 性 能 ， 鉴 于 上 述 的 控制 规则 不 能 获得 准确 
无 误差 的 轨迹 跟踪 ， 可 通过 在 控制 结构 中 加 入 前 馈 项 的 方法 ， 消 除 静 态 误差 并 
提高 轨迹 跟踪 精度 。 
本 文采 用 的 PD + 前 馈 滑 模 变 结构 控制 框图 如 图 4-1 所 示 158] 。 
对 应 图 4-1 的 总 控制 律 为 
T =T feedforward Кре -K è +7, (4-15) 























式 中 的 前 馈 项 为 
T feedforward = M( qd ) qa + С( qas qa ) Va + G( qa) (4- 16 ) 


第 4 章 机 器 人 研磨 自由 曲面 的 轨迹 跟踪 控制 策略 51 


А 机 器 人 [74 
各 关节 = 2 





т; 


" 
- T feedforward 
d, Mo (4a) +0) 
| 村 
Hy, ,d,) 
| 
| 
> П 
Яа 


qa 一 人 x)= 


图 4-1 PD + 前 馈 滑 模 变 结构 控制 框图 


















































从 而 控制 律 为 
7 =My (qa) a + Сб дай) da + GoCq4) -Ке-К„ё+т, (4-17) 
对 于 本 文 的 5 APE ALAA, DIENEN (4-6) 可 表示 为 
M(q)q+H(q,q) =т (4-18) 
Н(4,4) =C(q,4)¢+G(q) +F(q) +d(q,q,1) (4-19) 


AP, F(q) REA, dq, Q, 1) 表示 外 界 干扰 ， 其 他 具体 参数 意义 描述 
见 前 述 。 定 义 。=g -qa， 并 将 控制 律 式 (4-15) 代入 式 (4-18) 与 式 (4-19) 
中 ， 可 得 到 闭环 的 动力 学 方程 


M(q)e+C(q,q)@=A-K,e -K,e +7, (4-20) 
A = T feedforward 一 [M( qa) 4a +C(qq 4a) dà +G(qu) +F(q) +d(q,q,t) | 
(4-21) 


2 (4-20) 和 式 (4-21) 中 A 为 各 类 不 确定 性 的 集合 ， 如 果 摩 擦 力 F(9) 
及 外 界 干扰 d(q, q, 1) 的 影响 被 忽略 ， 且 系统 的 参数 误差 不 存在 ， 只 需要 将 





T feedforward = M ( qa) 4a +C(qa,qa)qa + GC qa) (4-22) 
此 时 ， 不 确定 项 A =0， 但 是 不 存在 这 样 的 理想 情况 ， 需 要 设计 T, 以 补偿 
不 确定 项 A。 
采用 前 馈 控 制 律 式 (4-17) ， 则 不 确定 项 A 为 
А =Trecatorward 7 (Maa) da + C( dada) da * GV aqu) +F(4) *d(q,3,0] 
-[Mo(qu) - M(q) 144 +[С(ча,й4)44—С(я,4)4д] + 
[ б\д) - G(q) ] -Е(9) -4(4,@,) (4-23) 
本 文 不 加 证 明 ， 基 于 如 下 假设 ， 应 用 控制 理论 进行 研究 。 
1) 期 望 轨迹 q 满足 


sup || ga ll Sco, sup | qa | Sei, sup |4 | «о; IEP су, сү, о 20 
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2) 惯性 矩阵 有 : |M) | <k,, Mola) | So, FPE EIER 

3) 1<(9,4) 1 Sk, lal, 1С, (9,4) | Sko 141 Skoci, AP E, 
ko 为 正常 数 。 

4) 对 重力 项 有 : |G) 1 Sk, I Go(qa) | Sko, AP ko, kyo WIE 

5) 对 摩擦 力 项 有 : || F(q) || <a+b | qi, Жа, b 为 正常 数 。 

6) 对 干扰 项 有 : [|4(4,д,) | Sdo +d lal +4 141, HP do, d, 
d, 为 正常 数 。 

滑 模 函 数 定义 为 

s=etAe (4-24) 

其 中 , A ЖЕ АИЯ , A SAI, A 50, 

结合 补偿 控制 器 设计 思想 ， 利 用 约束 函数 mn， 设计 一 个 具有 时 变 边界 层 的 
滑 模 补偿 控制 律 如 下 : 





-n(x,):sg(s) (s 2e(t)) 
e -ED (neg esc) m 
式 中 滑动 变量 * =¿ Ae, A20, e(t) >0 是 待定 的 边界 层 厚度 函数 。 可 
适当 地 选取 s(t) ， 对 控制 精度 加 以 保证 ， 从 而 使 控制 力矩 光滑 。 
定理 4-1 对 机 器 人 动力 学 方程 式 (4-18) 和 式 (4-19)， 采 用 式 
(4-17) 的 控制 结构 ， 则 滑 模 补偿 控制 律 式 (4-25) 使 得 闭环 的 动力 学 方程 式 
(4-21) 达到 最 终 一 致 有 界 稳定 0581 。 
证 明 : HX Lyapunov РА 27 
V =0.5[e"(K, + АК, )e +ëT Me] +¿TAMe =0. 5x" Px (4-26) 
š e K,+AK, AM 
AU, х= jme Р p M 
只 要 适当 选择 较 小 的 常数 和， 根据 矩阵 理论 可 知 ， 就 可 以 保证 矩阵 书 正 
KE o 
XIX (4-26) 求 导 ， 并 代入 闭环 的 动力 学 方程 式 (4-21), ， 可 得 
А АК, 0.5A(C - M) 
me -M)  K,-AM 
-x'Qx +SIA +s Tç (4-27) 
AK 0. 5A(C- M) 


S B p 
ANg Ё SA(C'-M) | K,-AM 





| A - AI 50, 





) *t51A +517; = 


| 要 使 O >0 必须 有 
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K,- AM >0 
AK(K, -AM) -0. 254° (C - M) (C- M) >0 
只 要 选择 足够 大 的 K, MK, ИЛЕМ A, ERRERA RIER, 
从 而 有 Q >0。 再 将 滑 模 补偿 控制 律 式 (4-25) 代入 式 (4-27) 得 
VS -х'Ох+ |slm- 15 ll m= -x Qr<0 (lsl=e(i)) 

















! j 4-28 
б атон |а Паб, de) casa seo 220) 
Y Isl ee А, 式 (4-28) 第 二 项 有 
V< -»10» eei) Uo. Cl sl <e(t)) 
欲 使 了 <0， 须 有 
x Qx > s (1) 05D. (4-29) 
根据 关系 式 
А (O) 112 T QA (Q) 151° (4-30) 
式 中 ，A， 表示 最 小 特征 值 ，A，, 表 示 最 大 特征 值 。 
H Apa (Q) lx E? >) Ch m, ў «0. ERENT 
e(t) (x,t) Y? И 
| х || > (£u. ) r(t) (4-31) 


取 B(7) 为 中 心 在 x=0， 半 径 为 r(1) 的 闭 球 ， 在 闭 球 B(r) 外 面 AV <0, 
所 以 只 要 wx(1) €B(r), 有 V<0, 在 有 限 的 时 间 7 内 ，x(t) 达 到 闭 球 的 边界 。 
取 t =t +T (wo 为 任意 初始 时 刻 ) 为 系统 状态 达到 闭 球 边界 的 时 间 ， 则 当 rz 
UHT, xG) seB(r)。 根 据 最 终 一 致 有 界 的 定义 ， 可 得 采用 滑 模 补偿 控制 律 式 
(4-25) 时 ， 闭 环 系统 为 最 终 一 致 有 界 。 


4.3 仿真 研究 


针对 式 (3-7) 研磨 工具 头 的 期 望 轨 迹 


x = —rsinwt 
y =0 
z= —r(1 -cosot) 





TE: r=60mm, Ж TAKA RIA, Hi AIM 20s, ARE o = 
1°/8, 
设 控制 参数 为 : k, = 3000, А, =230。 仿 真 结果 如 图 4-2、 图 4-3。 
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关节 位 移 /mm 


关节 转角 /rad 


关节 位 移 /mm 


—40F 









一 一 ШШ 
- 期 望 轨迹 












6 8 10 12 14 16 18 20 
时 间 /s 
a) 第 1 关节 期 望 轨迹 与 跟踪 轨迹 


一 一 БЕ 
ue 期 望 轨迹 











6 8 10 12 14 16 18 20 
时 间 /s 
b) 第 3 关节 期 望 轨迹 与 跟踪 轨迹 


一 一 跟踪 轨迹 
sius 期 望 轨迹 





Al 4-2 


1 
6 8 10 12 14 16 18 20 


时 间 /s 
с) 第 5 关节 期 望 轨迹 与 跟踪 轨迹 


各 关节 期 望 轨迹 与 跟踪 轨迹 
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时 间 /s 
с) 第 5 关节 误差 


图 4-3 各 关节 误差 


жож F EE DRE Л. Пу 2 JP WJ 


在 自由 曲面 的 研磨 精 整 加 工 过 程 中 ， 既 要 求 研磨 工具 按 着 期 望 的 路 径 规划 
完成 进 给 运动 ， 同 时 还 需要 保持 研磨 工具 头 与 加 工区 域 之 间 恒 定 的 作用 力 。 因 
此 ， 需 要 分 析 机 器 人 研磨 过 程 的 力 控 制 情 况 ， 本 文 的 机 器 人 柔顺 控制 是 位 姿 受 
限 的 力 / 位 混合 控制 问题 。 在 前 述 运动 规划 与 轨迹 控制 问题 的 基础 上 ， 本 章 主 
要 论述 研磨 过 程 中 力 的 控制 问题 。 根 据 前 面 的 叙述 可 知 ， 力 的 控制 方法 分 为 两 
种 。 第 一 种 方法 是 混合 力 / 位 控制 ， 依 据 接 触 运 动 时 位 置 与 力 信号 的 正 交 原理 ， 
实现 机 器 人 末端 的 接触 力 和 位 置 的 跟踪 控制 ， 即 力 控制 是 沿 着 受 约 东方 向 
(自然 约束 和 任务 约束 ) 进行 ， 位 置 控制 沿 着 非 约 束 方向 进行 。 为 了 克服 干 
扰 ， 位 置 控制 要 求 相 对 高 的 伺服 刚性 ， 而 力 控制 则 要 求 较 低 的 刚性 ， 以 保证 与 
环境 接触 时 的 柔顺 性 。 第 二 种 方法 是 阻抗 控制 ， 通 过 建立 机 器 人 末端 作用 力 与 
位 置 偏差 之 间 的 动态 关系 ， 对 研磨 工具 末端 作用 力 的 控制 是 通过 控制 机 器 人 位 
移 来 实现 的 ， 可 以 在 不 改变 机 器 人 位 置 控 制 器 的 前 提 下 实现 力 控制 。 混 合力 / 
位 控制 方法 理论 明确 ， 但 付 诸 实施 难 ; 阻抗 控制 具有 自由 运动 和 约束 运动 之 间 
的 转换 适应 性 强 的 特点 ， 具 有 良好 的 通用 性 和 应 用 前 景 。 


本 文 图 2-1 的 自由 曲面 研磨 精 加 工 系统 中 ，5 - TTRRT 机 器 人 具有 较 小 刚度 
的 研磨 工具 ， 以 及 该 工具 头 通过 被 动 结构 的 弹簧 与 机 器 人 本 体 相 固 联 的 结构 特 
点 ， 机 器 人 系统 的 被 动 结构 存在 于 研磨 工具 部 分 。 由 于 存在 主动 结构 与 被 动 结构 
的 情况 ， 为 了 弥补 机 器 人 动力 学 模型 和 环境 的 不 确定 性 与 提高 对 期 望 力 的 跟踪 能 
力 ， 应 用 阻抗 控制 法 实现 主动 与 被 动 相 结合 的 柔顺 力 控 制 。 阻 抗 控制 的 位 置 内 环 
采用 第 4 章 的 研磨 机 器 人 轨迹 跟踪 控制 系统 ， 在 本 章 建 立 的 阻抗 控制 作为 力 外 
环 ， 构 建 基于 位 置 的 阻抗 控制 ， 从 而 实现 研磨 机 器 人 系统 的 柔顺 控制 。 


5.1 阻抗 控制 概述 








5.1.1 机 器 人 阻抗 控制 


机 融 人 阻抗 控制 是 以 “ 弹 千 一 质量 一 阻尼 ”模型 对 机 器 人 末端 力 /位 置 的 
控制 进行 等 效 处 理 ， 为 达到 控制 目的 ， 需 要 调整 机 器 人 末端 位 置 与 接触 力 的 关 
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系 ， 通 过 任意 调节 惯性 、 阻 尼 、 刚 度 等 参数 来 实现 。 

基于 力 的 阻抗 控制 和 基于 位 置 的 阻抗 控制 是 两 种 不 同 的 阻抗 控制 实现 方 
So 

基于 力 的 阻抗 控制 由 外 环 的 阻抗 控制 和 内 环 的 力 控制 组 成 ， 为 了 得 到 期 户 
阻抗 模型 ， 作 用 在 机 器 人 未 端的 参考 力 К, 由 阻抗 控制 外 环 计算 得 到 ; 对 于 机 
器 人 与 环境 之 间 的 实际 作用 力 跟 踪 的 期 望 接触 力 由 内 环 的 力 控制 咒 实 现 。 

基于 位 置 的 阻抗 控制 由 外 环 的 阻抗 控制 和 内 环 的 位 置 控制 组 成 ， 位 置 修正 
量 由 阻抗 控制 的 外 环 生成 ， 参 考 位 置 、 位 置 的 修正 量 和 实际 位 置 输入 到 内 环 位 
置 控制 器 中 ， 完 成 实际 位 置 对 期 望 位 置 的 跟踪 ， 以 实现 机 器 人 与 环境 接触 作用 
模型 成 为 期 望 阻抗 模型 。 


5.1.2 阻抗 控制 模型 


根据 Hogan "提出 的 阻抗 控制 概念 可 知 ， 对 力 和 位 置 偏差 的 期 望 关系 进 
行 调节 是 阻抗 控制 的 关键 ， 建 立 的 期 望 关系 称 为 目标 阻抗 ， 如 式 (5-1) 所 
不 。 








F-F,-Z(X-X,) (5-1) 
式 中 , F. FQ, X. X, 分 别 为 机 械 手 与 环境 的 接触 力 、 期 望 力 、 位 置 和 期 
望 位 置 。 
在 质量 一 弹 筑 一 阻尼 系统 表示 的 阻抗 如 图 $- 1 所 示 。 机 械 手 末端 与 环境 相 
互 接触 产生 作用 力 ， 发 生 形 变 。 对 于 机 器 人 工具 未 端 位 置 偏离 期 望 轨迹 的 差 和 
末端 与 外 界 间 的 相互 作用 力 ， 建 立 的 目标 阻抗 可 用 如 下 二 阶 微分 方程 描述 ， 式 
(5-2) 给 出 了 常用 的 目标 阻抗 三 种 数学 表达 形式 1159]。 





M,X +B, -K,(X-X,) = -F (5-03) 
MX +В,(Х-Х,) +К,(Х-Х,) = -F (5-2b) 
Mj - X) +B,(X-X,) *K(X-X) = -F (26) 


式 中 XQ). XG), X, (4) ——BL88 A Aan SS BE AR SE T] ; 











X, X, X—x. y, z063 xl 的 机 器 人 终端 位 置 、 速 度 
和 加 速度 向 量 ; 
M,, Bj, К,—3х3 的 目标 惯量 矩阵 、 目 标 阻尼 矩阵 和 目标 
刚度 矩阵 ; 
Е X. у, 2 方向 3 х1 的 机 器 人 工具 端 与 环境 接 


触 时 ， 环 境 作用 给 机 器 人 工具 端的 力 向 量 。 
为 达到 力 跟 踪 的 目的 ， 在 上 面 的 研究 基础 上 ， 采 用 实际 接触 力 与 期 望 接触 
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Ba 
Ba F 
s s. 
а а Ka Mı 
c Ee 8 


a) 式 (5-2a) b) 式 (5-2b) с) XX (S—2o) 





www 
> 
a 
局 


图 $-1 s& (5-2) 物理 模型 


力 之 差 ej =F, F FUR (5-2) 中 的 环境 作用 力 严 ， 作 为 目标 阻抗 模型 的 驱 
动 信 号 ， 得 到 新 目标 阻抗 模型 为 








M,X +В,Х +К,(Х-Х,) =e, (5-3а) 

M,X+B,(X-X,) +K,(X-X,) =e, (5-3b) 

M,(X -X,) +В,(Х-Х,) +К,(Х-Х,) =e, (5-3c) 
QE=X-X,, ЖН (5-30) 表达 的 目标 阻抗 模型 为 

ME +B Ë + KE =e, (5-4) 


TE ER E JL Aes RPA tt r n] а EE, НИЕ М. HER 
BEE FEM Bj、 目 标 刚度 和 矩阵 Ky 和 环境 刚度 矩阵 K, 一 般 都 可 取 为 对 角 正 定 矩 
阵 ， 因 此 可 以 进行 简化 ， 只 考虑 机 器 人 操作 空间 某 一 维 的 情况 ， 分 析 机 顺 人 笛 
卡 儿 坐标 系 任意 一 坐标 轴 方 向 的 阻抗 控制 ， 可 用 上 f. m. b. k F, F. 
М, Bi, К, Jf ezx-x, WK (5-4) BA 

me+bé+ke=f. -f (5-5) 

在 自由 空间 和 约束 空间 中 分 别 考察 机 器 人 的 两 个 运动 。 因 自由 空间 中 ， 研 

磨 工具 末端 接触 力 f=0， 则 有 











me+bé+ke=f, (5-6) 
AP, AAS MMS, MRE SH f 为 0， 工 具 端 的 驱动 力 
决定 了 期 望 力 ， 机 絮 人 将 与 环境 保持 接触 ， 使 机 右 人 对 环境 施加 力 。 


5.2 机 费 人 研磨 工具 端 与 环境 的 等 效 模型 


5.2.1 等 效 模型 的 建 江 


研磨 机 器 人 在 自由 空间 中 不 与 环境 发 生 作 用 ， 是 一 个 独立 的 受 控 对 象 ， 对 
人 研磨 工具 末端 的 控制 是 单纯 的 位 置 控 制 。 而 当 人 研磨 工具 末端 与 环境 接触 后 ， 由 
于 受到 环境 的 约束 ， 此 时 的 机 器 人 研磨 工具 末端 不 再 是 一 个 独立 的 受 控 对 象 ， 
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研磨 工具 终端 与 环境 构成 了 一 个 综合 动态 系统 。 在 众多 的 研究 中 ， 线 性 弹簧 系 
统一 般 被 当 作 机 械 手 末端 与 环境 接触 的 简化 模型 ， 即 如 式 (5-7) 所 示 的 模 
型 。 但 是 在 研磨 加 工 中 ， 考 虑 材料 去 除 装置 〈 研 磨 工具 ) 与 环境 (工件 表面 ) 

















接触 的 瞬 态 过 程 情况 ， 应 该 同时 考虑 位 置 项 的 作用 与 阻尼 项 的 影响 ， 此 时 可 以 
把 接触 模型 简化 为 式 (5-8) 所 示 的 “阻尼 一 弹 答 ”系统 。 结 合式 (5-3c) 的 
研磨 工具 终端 与 环境 的 接触 模型 如 图 5-2 所 示 。 
F-K,(X-X,) (3-7) 
F-B(X-X,)) +K,(X-X,) (5-8) 
阻抗 控制 器 环境 





й 
А 

















图 5-2 阻抗 控制 机 器 人 和 环境 的 简化 模型 


图 5-2 中 ， 在 只 考虑 笛 卡 儿 坐 标 系 某 一 轴 方 向 的 情况 下 ， 用 x,、x 和 wx, 分 
别 表示 研磨 工具 末端 在 一 个 坐标 轴 方 向 上 的 轨迹 参考 位 置 、 实 际 运行 位 置 和 环 
HME, 用 了 表示 研磨 工具 末端 与 环境 的 参考 接触 力 ， 用 一 线性 弹 答对 环境 
进行 等 效 处 理 ， 人 研磨 工具 上 的 力 传 感 右 与 环境 的 等 效 刚度 为 k.， 则 有 
0 (x<x.) 


LE 和) (xXx) 





了 (5-9) 


5.2.2 人 研磨 机 器 人 系统 的 刚度 系数 K, 


因 本 文 的 研磨 机 器 人 系统 含有 主动 结构 (机 器 人 本 体 ) 与 被 动 结构 (DE 
ЖТА) 两 部 分 ， 所 以 研磨 机 器 人 系统 刚度 为 [161 
K, =K, OK, (5-10) 
式 中 Ki 一 一 机 器 人 本 体 的 主动 部 分 刚度 ; 
K, 一 一 人 研磨 工具 的 被 动 部 分 刚度 ，; 
久 一 一 表示 两 个 矢量 按照 某 种 测度 求 和 。 
从 图 5-3 中 可 知 ， 机 器 人 系统 所 受 的 力 来 自 研磨 工具 端 部 与 工件 表面 之 间 
的 作用 力 ， 也 是 系统 的 被 动 部 分 所 受 的 外 力 。 鉴 于 机 器 人 的 本 体重 量 相对 于 吸 
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研磨 工具 头 

















图 5-3 ”研磨 机 器 人 系统 被 动 部 分 的 结构 、 刚 度 与 变形 


附 吸 盘 产 生 的 吸附 力 足 够 大 ， 本 文 假设 机 器 人 的 吸盘 和 工件 表面 吸附 后 ， 机 天 
人 相对 于 研磨 工具 末端 是 刚性 的 ， 系 统 被 动 部 分 受到 作用 力 后 ， 这 些 刚性 位 移 
使 被 动 部 分 〈 研 磨 工具 的 弹簧 ) 产生 弹性 变形 。A, 表示 系统 主动 部 分 使 研磨 
工具 产生 的 位 移 ，4， 表示 工件 使 研磨 工具 弹簧 产生 的 位 移 ， 两 部 分 共同 使 研 
磨 工具 产生 的 变形 为 A。。 

在 本 文 的 机 器 人 基 坐 标 系 中 ， 设 研磨 工具 未 端 在 yOz 平面 的 两 个 方向 上 刚 














Кы К, 
i I ч «| = ü l 
式 中 K, 一 一 y 方向 的 单位 力作 用 引起 的 y 方 向 相对 于 研磨 工具 前 端 中 心 的 变 
JE; 
KK 一 一 y 方向 的 单位 力作 用 引起 的 z 方 向 相对 于 研磨 工具 前 端 中 心 的 变 
Ж; 
KK, 一 一 z 方向 的 单位 力作 用 引起 的 y 方 向 相对 于 研磨 工具 前 端 中 心 的 变 
JE; 
有 .一 一 :方向 的 单位 力作 用 引起 的 z 方 向 相对 于 研磨 工具 前 端 中 心 的 变 











形 。 
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在 图 5-3 中 ,研磨 工具 末端 相对 于 机 器 人 终端 的 弹性 变形 量 》 
ач “һм (5-12) 
Ficos т] + Е, зіп m K, Kp jJ Ay 
在 本 文 的 机 器 人 系统 中 ， 在 加 工 平面 的 z 方 向 和 YY 方向 布置 两 个 力 传 感 
ar, AAS SNM EH FA К 


yn 





FY =F, ` cos TJ + F, * sin T 
ч 4 (5-13) 
Fan =F, ° cos n- Е, siny 


联 立 式 (5-12) 可 得 
F zn Кы k; A, 
- (5-14) 
F yn ky, ky A ye 


在 本 文 设计 的 研磨 机 器 人 系统 中 ， 弹 性 变形 只 在 研磨 工具 末端 产生 ， 其 他 
部 分 产生 的 是 刚性 位 移 。 人 研磨 工具 末端 的 轴 向 和 切 向 两 个 方向 产生 弹性 变形 ， 
因 轴 向 力 产生 的 切 向 变形 和 因 切 向 力 产 生 的 轴 向 变形 相对 较 小 ， 可 以 忽略 。 设 
研磨 工具 未 端的 轴 向 刚度 系数 为 瓦 ， 切 向 刚度 系数 为 如， 可 得 


F, k, 0 Ay 
= (5-15) 

"n Ë . 
对 于 研磨 工具 末端 的 刚度 系数 设计 ， 主 要 是 被 动 部 分 的 弹性 变形 量 远大 于 
主动 部 分 的 运动 误差 ， 为 避免 某 方向 上 的 刚度 过 大 ,让 上 六 WD, 取 值 尽量 相 


近 ， 结 合 图 5-3 可 得 
pa d Ju] TS 
F pcos А 0 k. jJl Are 
Pu +7) Min E (5-17) 
A,, cos(A +m) sin(A +7) AA, 
[HE +7) кы Ed eu (5-18) 
F, cos( À +7) sin(A +1) AF,, 
将 式 (5-17) 和 式 (5-18) 代入 式 (5-15) 可 得 
A +n)  -cos(A Ed ng = E 0 (“9 +n)  -cos(A e) 
cos( À +7) sin(A +7) AF,, 0 А, J\cos(A +) sin(A +7) AA, 
(5-19) 


) ÆR (5-19) 等 号 两 边 各 左 条 








sin(A*9) -cos(À +2) 
b da eee +) sin(A +7) 
4-!， 再 联 立 式 (5-14) 可 得 
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k. k; k, 0 
=A `! (5-20) 
Е... k; 0 k, 
将 4 代入 式 (5-20) 可 得 
k „=k cos” (A+n) +k,sin? (А +m) 


k, = (Е, - Е,) sin(A +1) cos(A +?) 





(5-21) 
ky =k sin? (А +1) + k,cos? (A +7) 


k, = (Е, —k,) sin(A +1) cos(A +7) 

由 于 系统 的 被 动 结构 部 分 是 弹性 变形 全 部 集中 发 生 的 位 置 ， 因 此 可 以 用 被 

动 部 分 的 位 置 刚 度 系数 K, 代 蔡 系 统 的 位 置 刚 度 系 数 K, ， 即 
K, K, 
rome 


如 果 给 定 了 名 ALK, BORD 20942 К. 





(5-22) 

















5.3 基于 位 置 的 阻抗 控制 


5.3.1 基于 位 置 的 阻抗 控制 


图 5-4 表示 了 本 文 的 研磨 机 带 人 系统 加 工 过 程 示意 图 。 


机 器 人 未 端 工具 
待 加 工 表面 (L) 







加 工 前 理论 表面 (2.) 
已 加 工 表面 (P) 


期 望 得 到 的 位 置 Cao) 


图 $-4 研磨 加 工 过 程 示意 图 


基于 位 置 的 阻抗 控制 由 外 环 的 阻抗 控制 和 内 环 的 位 置 控制 组 成 ， 位 置 修正 
量 由 阻抗 控制 的 外 环 生成 ， 参 考 位 置 、 位 置 的 修正 量 和 实际 位 置 输入 到 内 环 位 
置 控制 器 中 ， 完 成 实际 位 置 对 跟踪 期 望 位 置 的 跟踪 ， 以 实现 机 器 人 与 环境 接触 
作用 模型 成 为 期 望 阻 抗 模 型 。 本 文 的 位 置 控制 内 环 采 用 上 一 章 的 PD + 前 馈 型 
变 结构 补偿 控制 器 ， 阻 抗 控 制 的 效果 取决 于 位 置 控制 器 的 精度 。 在 本 文 建立 的 
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基于 位 置 稍 卡 儿 阻 抗 控 制 中 ， 安 装 在 研磨 工具 上 的 压力 传感器 实施 研磨 工具 与 
环境 之 间 的 接触 力 测量 ， 该 力 被 反馈 给 目标 阻抗 控制 器 后 ， 目 标 阻抗 控制 器 产 
生 一 个 位 置 修正 向 量 e = (e,，e,，e,) ， 位 置 修正 向 量 满足 下 式 。 





-Е=М,е+Вё+ Ке (5-23) 
因此 在 频 域 中 阻抗 函数 表示 为 
ey A (5-24) 


M,s° +В +K; 
X (5-24) "PAY Ma, Ba, K, 都 是 对 角 正 定 和 矩阵 , 式 (5-24) 相当 于 二 
阶 低 通 滤波 器 ， 可 以 对 (s) 中 每 一 个 元 素 实现 二 阶 低 通 滤波 ， 通 过 对 每 一 个 
F 的 滤波 ， 得 到 滤波 后 的 位 置 修正 向 量 e。 位 置 修正 向 量 e 与 机 器 人 轨迹 规划 
产生 的 参考 位 置 向 量 鲜 , 相 加 ， 得 到 位 移 控制 命令 X, = (ху, ул. za)"; 
Ху=Х,+е (5-25) 
当 研磨 工具 未 端 在 自由 空间 中 运行 时 ， 与 环境 不 发 生 接触 ， 受 到 的 外 界 作 
用 力 F=0， 此 时 对 应 的 位 移 修正 问 量 e=0， 由 式 (5-14) 18] X, =Х„ 
当 人 研磨 工具 末端 在 约束 空间 中 运行 时 ， 工 具 末 端 与 环境 接触 后 ， 若 假设 位 
置 控 制 器 精确 且 无 误差 ， 即 站 = 对,， 则 有 
е=Х-Х, (5-26) 
基于 位 置 的 阻抗 控制 方法 如 图 5-5 Hrs, ЕЛАК НУКЕ ХХ, = HZ 
ZME X, 与 位 置 修正 向 量 e (工具 末端 所 受 环境 外 力 FF 经 过 阻抗 滤波 器 得 到 
的 ) 相 加 得 到 的 。 然 后 将 X, 通过 道 运动 学 运算 L-! (X), ， 得 到 关节 空间 的 
期 望 关节 角度 9,， 将 0, 减 去 实际 通过 反馈 得 到 的 关节 角度 9， 可 得 到 关节 位 
置 误差 9,。 当 位 置 控制 器 精确 时 ， 关 节 位 置 控制 器 的 输入 Ө, =0， 经 过 正 运动 
学 运算 ,达到 对 =L(0)0 =L(0,)90, =X, Н, 



































+ X, 4 
Ja (m) (А) 














ju 
位 置 控制 环 
2 š E 


图 5-5 ”基于 位 置 的 阻抗 控制 (1 
对 于 研磨 工具 末端 不 与 环境 接触 的 自由 运动 情况 ，F。 =0， 此 时 阻抗 模型 
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M,(X-X,) +B,(X -X,) +K,(X-X,) =0 (5-27) 
式 (5-27) 中 ,在 有 较 大 加 速度 的 高 速 运动 或 者 会 产生 冲击 力 的 运动 情 
况 下 ， 机 器 人 理想 惯性 矩阵 М 产生 的 影响 较 大 ; 在 中 速 运动 或 存在 较 强 干扰 
的 情况 下 ， 机 器 人 理想 阻尼 和 矩阵 Bj 产生 的 影响 较 大 ; 平衡 状态 附近 的 低速 运 
动 时 ， 机 器 人 的 理想 刚度 矩阵 K, 产生 的 影响 较 大 ; 可 以 通过 主动 控制 来 调节 
阻抗 控制 参数 来 获 到 不 同 的 目标 阻抗 Ma, Bas Ka 都 分 别 包 含 了 物体 固有 的 
和 主动 控制 所 带 来 的 部 分 。 对 于 式 (5-27) 表示 的 目标 阻抗 系数 ， 参 考 文献 
[160, 162] 详细 地 论述 了 由 自由 空间 向 约束 空间 过 渡 时 的 稳定 性 问题 。 
B, 


é, = 
2 v К.М, 














(5-28) 





£,20.5 ( /1 +K-1) 
5.3.2 阻抗 控制 中 稳 态 力 误 差分 析 
由 式 (5-7) ATA, 4 хх, 时 ， 则 有 
x, =f / k, + x, (5-29) 
工作 中 参考 位 置 x, HEX RR, MA x, =%,=0. Af, 为 恒 量 ， 则 
有 广 = 广 =0, HR (5-29) FLASK (5-3c) ， 则 所 定义 的 目标 阻抗 模型 为 
m eg * bé, (k +k, ep=hf, -k k(x, =x) 





(5-30) 
e =f, -f 
稳 态 力 误差 为 7 
+ 
es = "ie pep eS) ы а (5-31) 
式 中 ы=@ eit 1 IE, 
稳 态 接触 力 为 

fo =f, -e =k (E +x, —Х„) (5-32) 
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М (5-31) 可 知 ， 稳 态 力 人 与 参考 力 矿 及 参考 轨迹 位 置 x, 的 函数 关 
系 ， 在 参考 位 置 满 足 式 (5-29) HJ, Жїк 711x223 e, =0。 当 环境 位 置 x, 和 环 
境 刚度 到 精确 可 知 时 ， 实现 无 误差 的 参考 力 轨迹 跟踪 的 条 e Ir E x, Е, + 
x, 来 选择 参考 轨迹 位 置 x, 。 然 而 ， 环 境 刚 度 k. 和 环境 位 置 x, 的 精确 值 很 难 在 
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实际 操作 中 获得 。 通 常 取 估计 值 xz。 Ak, We Ax, Ak, 为 实际 环境 位 置 和 环境 
刚度 与 假设 的 环境 位 置 和 环境 刚度 间 的 差 ， 则 有 


Ax, =х,-х, 








i (5-33) 
Ak, =k, - b, 
式 中 x, 8 一 一 实际 的 环境 位 置 和 环境 刚度 ; 
x。 8 一 一 环境 位 置 和 环境 刚度 的 估计 值 。 
使 用 x, =f/k, +x, 定义 的 参考 位 置 时 的 稳 态 力 误差 为 
Е ерд АВ 
a IE eX, 一 j^" (5-34) 





采用 直接 对 环境 参数 进行 估计 的 方法 ， 可 解决 环境 刚度 、 环 境 位置 不 确定 
引起 的 力 误差 。 由 于 环境 刚度 通常 很 大 ， 由 式 (5-34) 可 知 ， 即 使 取 很 小 
的 环境 位 置 偏差 Ax。， 力 控制 误差 值 也 会 很 大 。 若 使 阻抗 控制 能 完成 精确 的 力 
控制 任务 ， 实 时 对 环境 参数 在 线 估 计 是 最 直接 的 办 法 。 


5.3.3 调整 阻抗 参数 的 仿真 研究 


为 得 到 阻抗 参数 变化 时 对 系统 性 能 的 影响 ,采用 上 述 的 基于 位 置 的 阻抗 控 
制 方法 。 针 对 式 (3-7) 给 出 的 期 望 轨迹 ,分 别 选取 不 同 的 阻抗 参数 得 到 的 仿 
真 结果 如 图 5-6 ~ 图 5-10 所 示 ， 左 图 为 力 跟踪 曲线 ， 右 图 为 位 置 曲线 。 

图 5-6 +, М, =10kg、B, =500N - s/m, K, =3000N/m， 从 图 中 可 以 看 
出 ， 操 作 辟 控制 系统 由 自由 空间 到 约束 空间 过 渡 过 程 比较 稳定 ， 力 的 稳 态 波动 
很 微小 。 












































20r 70 
18 
16| 60 
14r f 50 у 
上 / 
< 2 | E 40 / 
R 10 | a / 
8r | ie 30 j 
6 20: | 
4 
A 10 / 
" 1 L L L L L п 1 j lA А 
О 2 384 5 $ y S 9 US TU 
时 间 /s 时 间 /s 
引力 跟踪 曲线 b) 位 移 曲 线 


图 5-6 基于 位 置 的 阻抗 控制 仿真 图 (一 ) 
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图 5-7 中, Mj -100kg, B 








1=500N + s/m, K, = 





=3000NMm， 从 图 中 可 以 看 
































出 ， 力 的 稳 态 值 处 于 小 波动 状态 ， 是 由 于 M, 取 值 过 大 ， 在 由 自由 空间 到 约束 
空间 过 渡 过 程 中 ， 系 统 变 得 不 稳定 。 
70 
60! 
50} - 
B 40 
№ зо 
20! 
10} / 
0 J 2 s ¿4 S @ дю IÑ ш 
时 间 /s 时 间 /s 
a) 力 跟 踪 山 线 b) 位 移 曲 线 
图 5-7 基于 位 置 的 阻抗 控制 仿真 图 (二 ) 





图 5-8 中 , M, =10kg, B, =80N · 














s/m, Ку = 


=3000N/m, А A We 





























出 ， 力 表现 出 不 稳定 现象 ， 是 由 于 В, 取 值 过 小 ， 在 由 自由 空间 到 约束 空间 过 
Be, AMM AMA. 
20 70 
18. 
60 
16 
14} p 50 
z | | В 40 
S | È 30 
6 | 20 
| | 10; 
0 1 2 54 567 8 9 10 G 3 2 322 S 6 ОШ 
时 间 /s 时 间 /s 
a) JJ REK Ze b) (ee th £x 
5-8 基于 位 置 的 阻抗 控制 仿真 图 (=) 


图 5-9 中 , М, -10kg, B, 
`á K, 取 值 过 小 时 ,在 由 自由 空 
但 力 的 稳 态 值 较 好 。 

图 5-10 中 ， 不 能 实现 稳定 


AX (5-28) 的 条 件 ， 从 图 中 可 以 看 出 ， 系 统 由 自由 空 


=500N · 
x [RI 8| 29 а= 


的 阻抗 控制 ， 


s/m, K, =10N/m， 从 图 中 可 以 看 出 ， 
间 过 渡 过 程 中 ， 力 波动 范围 较 大 ， 





是 由 于 阻抗 参数 选取 不 当 ， 不 满 





s 间 到 约束 空 


3 间 的 过 渡 
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Rg E / 
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3 10 / 
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@ 12 эл 46 7 8 @ iQ @ be uomo «uu 8 8r 
时 间 /s 时 间 /s 
а) J) ERR MR b) 位 移 曲 线 
5-9 ”基于 位 置 的 阻抗 控制 仿真 图 (四 ) 
20 70 
18 60 
16 
14 50 2 x 
12. E 40 ы M X S 
£10 a 
ET {5 30 
6 20 
4 
2 10 
® 1 2 8 4 5 6 F 8 8 Q Q d 3 uu 5 6 7 8 9 10 
时 间 /s 时 间 /s 
a) JJ ERE II 28 b) 位 移 曲 线 
图 5-10 基于 位 置 的 阻抗 控制 仿真 图 (五 ) 








过 程 变 得 极 不 稳定 。 


由 上 面 的 仿真 结果 可 以 看 出 阻抗 控制 模型 参数 (Mu B. 


` Kı) 必须 根据 


任务 要 求实 时 调整 : 较 大 的 目标 惯性 矩阵 М, 将 会 对 环境 产生 大 的 冲击 运动 ， 
导致 轨迹 误差 较 大 ， 系 统 响应 慢 ; В, 越 大 系统 啊 应 越 慢 ， 超 调 量 越 小 ， 能 量 
消耗 越 大 ; K 值 越 小 ， 力 控制 稳 态 误差 越 小 ， 系 统 响 应 越 慢 ， 反 之 ， 轨 迹 跟 


踪 精 度 越 高 。 


5.4 模糊 阻抗 控制 





通过 上 述 阐述 与 仿真 分 析 可 知 ， 
抗 参数 。 针 对 目标 阻抗 的 参数 调整 ， 








阻抗 控制 的 动态 性 能 直接 取决 于 不 同 的 阻 
一 种 方法 是 采用 实时 测量 或 根据 环境 模型 
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计算 (k. b.) 的 值 ， 根 据 环境 的 变化 修改 (Ma, By. Ka) 进行 补偿 ; 男 一 
种 方法 是 采用 智能 控制 方法 ， 根 据 机 器 人 末端 位 置 和 接触 力 反馈 实时 调整 
(Mı, By. К), ， 改 变 环境 接触 力 与 位 置 偏差 的 关系 ， 牺 牲 一 定 的 位 置 偏差 ， 
就 可 避免 力 的 过 大 变化 ， 降 低 环境 变化 对 系统 动静 态 性 能 的 影响 ， 这 就 是 阻抗 
控制 的 鲁 棒 性 。 由 于 研磨 工具 与 环境 的 接触 过 程 中 ， 存 在 被 控 对 象 具 有 时 变性 
以 及 随机 干扰 情况 ， 即 环境 的 位 置 和 刚度 是 不 断 变化 的 情况 ， 采 用 固定 目标 阻 
抗 系数 的 控制 方法 往往 引起 大 的 碰撞 和 超 调 ， 不 能 满足 任务 的 要 求 。 一 些 学 者 
在 提高 阻抗 控制 的 鲁 棒 性 和 实时 性 方面 ， 通 过 引入 模糊 控制 或 者 神经 网 络 来 调 
整 目标 阻抗 系数 ， 成 为 目前 研究 的 热点 08 -16] 。 本 文中 对 目标 阻抗 的 调整 ， 
是 为 了 避免 对 待 加 工 曲 面 轮廓 误差 的 复 映 ， 以 保证 好 的 位 置 控制 性 能 。 调 整 位 
置 偏差 不 单纯 为 了 得 到 恒定 的 接触 力 ， 而 是 适当 放大 力 误 差 的 范围 ， 使 力 在 一 
个 给 定 的 范围 内 变动 ， 对 目标 阻抗 实施 智能 控制 。 


5.4.1 基于 模糊 逻辑 的 阻抗 控制 设计 


1. 控制 方案 设计 

由 于 竺 加工 工件 经 过 前 期 数控 加 工 后 ， 导 致 工件 表面 存在 形状 误差 ， 如 果 
继续 采用 恒 力 控制 ， 会 导致 加 工 后 表面 对 加 工 前 表面 轮廓 形状 的 复 映 。 为 了 实 
现 位 置 跟踪 ， 本 文 在 进行 基于 位 置 阻 抗 的 模糊 逻辑 控制 器 设计 时 ， 对 目标 阻抗 
参数 实行 实时 调整 ， 使 力 误差 的 范围 适当 放大 ， 模 拟人 手工 研磨 的 力 控制 情 
况 ， 能 主动 实现 对 被 加 工 工件 表面 变化 的 适应 ， 对 于 每 次 研磨 加 工 ， 为 了 取得 
最 合理 去 除 残 余 余 量 的 要 求 ， 环 境 的 变化 要 求 目 标 阻抗 参数 随 着 进行 调整 Ж 
文 对 阻抗 参数 进行 的 动态 调整 采用 的 是 模糊 控制 策略 ， 模 仿 人 的 决策 过 程 进行 
实时 调整 。 其 控制 框图 如 图 5-11 所 示 。 

2. 模糊 控制 器 的 设计 115] 

(1) 确定 输入 输出 变量 。 为 了 取得 高 质量 的 被 加 工 表 面 ， 实 现 仿 人 研磨 
加 工 的 主要 目的 ， 控 制 器 中 以 位 置 误差 和 误差 变化 量 为 模糊 控制 器 的 输入 。 通 
过 前 述 可 知 ， 目 标 刚度 系数 是 影响 加 工 质量 的 主要 因素 ,目标 阻尼 系数 В 
则 影响 系统 的 超 调 量 。 以 XK 和 B 的 变化 量 作 为 模糊 控制 系统 的 输出 ， 构 成 一 
种 双 输 入 双 输 出 模糊 系统 。 

(2) 模糊 变量 和 模糊 化 。 在 模糊 控制 器 中 ， 输 入 输出 变量 的 语言 值 均 被 
分 为 七 个 模糊 子 集 (NB. NM. NS, ZE. PS, PM, PB), ， 输 入 误差 论 域 为 
[-0.03, 0.03], ， 误 差 变 化 论 域 为 [-0.3,，0.3] ,输出 变量 论 域 为 [—800, 
800] 和 [ —500, 500], 

La, 5b] 为 输入 变量 论 域内 任意 变量 x 的 实际 值 区 间 ，[ a, 5] 区 间 的 精确 
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图 5-11 模糊 阻抗 控制 框图 59] 
量 转 换 为 区 间 [ — m, m] (m 为 正 整数 ) WEE x, 采用 以 下 公式 。 


2т[х = (a +b) — 2 


x' = < > (5-35) 
b-a 


比例 因子 k=2m/(b —a), «c > 表示 取 整 运算 。 同 理 对 于 输出 变量 比例 
因子 分 别 为 k=60、k。 =10, kap 2266, ky, =166。 
(3) 隶属 度 函数 。 式 (5-36) 为 梯形 隶属 度 函 数 j (х), Жа, Б. с, 


















































4 分 别 为 梯形 的 四 个 顶点 ， 如 图 5-12 所 示 。 

NB NM NS ZE PS PM PB 
| 
| 
| 
| 
| 
| 
| 
| 
| 
| 
| 
| 
| 
| 
| 
| 

-3 -2 4d 0 1 2 3 





图 5-12 隶属 度 函 数 [159] 
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MA (x) = max(min((x ~- a)/(b -a),l,(d -x)/(d-c))) (5-36) 
x (5-36) 展开 式 为 
0 


Xa 
(x-a)/(b-a) а<х<ьЬ 
Ba(x) = 41 b<x=<c (5-37) 
(d-x)/(d-c) с<х<4 
0 d < x 


(4) 模糊 规则 。 设 模糊 规则 集 R 由 一 组 “下 一 THEN” 模 糊 规则 构成 : 
R= (Ri QR P5) 

其 中 

Ку: IF e is A, and ec is Ву, THEN Ak is C, and Ар is Dj. 


R;: IF e is A; and ec is В;, THEN АЁ is C; and Ab is D;. 

R 代表 规则 库 中 的 第 j 条 规则 ; А, B. C. D, WERDER ZERE NIE, 
其 控制 规则 如 表 5-1 所 示 。 

(5) 模糊 推理 与 清晰 化 。 对 当前 各 变量 的 输入 值 为 e、ec， 输 出 值 上 大、0， 
采用 MIN - МАХ 合成 法 进行 模糊 推理 。 
Hye (2) = Ie Ce) A Си Cre (2) TT A V ias (у) A mC) | 
V Cates (a) AG) A G1 O sss (0) Anas O0] A pl 
[w An CO] OV Га, A (2) ] 

= (a, A а) A pz) (5-38) 

式 中 , 人 表示 隶属 度 ; aw。、a.。. 为 当前 值 与 规则 的 适 配 度 ， 即 规则 激活 的 程度 。 
采用 重心 法 解 模 糊 








M 








n 











> My И (2; ) 2; 
i=l 
zy = LL (5-39) 
> K, ( Zi ) 
i=l 
表 5-1 控制 规则 表 
AKAB NB NM NS ZE PS PM PB 
NB NM NS ZE ZE ZE ZE 
NB 
NB NB NB NB NS NS ZE 
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(Ж) 
АКАВ NB NM NS ZE PS PM PB 
NB NM NS ZE ZE ZE ZE 

NM 
NM NM NM NM ZE ZE ZE 
NB NS NS ZE ZE PS PM 

NS 
NM NM NS NS ZE ZE NS 
NB NB NS ZE PS PB PB 

ZE 
NB NB NS ZE PS PB PB 
NM NS ZE ZE PS PM PB 

PS 
PS ZE ZE PS PS PM PM 
NS ZE ZE ZE PS PM PB 

PM 
ZE ZE ZE PM PM PM PM 
ZE ZE ZE ZE PS PM PB 

PB 
ZE PS PS PB PB PB PB 























5.4.2 常规 阻抗 控制 与 模糊 阻抗 控制 的 仿真 研究 


为 了 比较 常规 阻抗 控制 与 模糊 阻抗 控制 的 差别 ， 取 式 (5-3c) 作为 目标 
阻抗 ， 实 现 式 (3-7) 表达 的 研磨 工具 头 期 望 轨 迹 ， 其 运动 学 与 动力 学 模型 ， 
结合 第 3 章 与 附录 中 的 计算 公式 ， 期望 力 取 为 15. 5N， 目 标 阻 抗 参数 如 下 


1 0 0 
M, = 1 ‚Ву = 250 ‚ Kı = 200 
1 250 200 


图 5-13 为 常规 目标 阻抗 参数 的 恒 力 跟踪 控制 方式 ， 图 5-14 为 加 入 模糊 控 
制 絮 的 模糊 阻抗 控制 加 工 结果 ， 图 中 曲线 7, 为 工件 加 工 前 的 表面 位 置 ， 曲 线 
Z, 为 加 工 后 的 期 望 位 置 ， 曲 线 Z 为 研磨 工具 头 的 实际 位 置 。 假 设 研磨 工具 的 
初始 位 置 为 0mm， 可 以 看 出 ,模糊 控制 方法 中 ， 较 小 的 值 减 小 了 力 超 调 量 ， 
系统 稳 态 误差 变 小 ， 稳 定性 好 ， 对 于 环境 参数 已 知情 况 下 的 力 跟 踪 控 制 能 够 更 
好 地 适应 ,但 增 大 了 系统 响应 时 间 。 
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a) 位 置 仿真 曲线 








b) 作用 力 响应 仿 趴 曲线 








с) 位 置 误差 仿真 曲线 
5-13 国定 目标 阻抗 系数 仿真 
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6.1 研磨 材料 的 去 除 模 型 














研磨 的 主要 目的 是 改变 表面 质量 ( 减 小 工件 表面 粗糙 度 值 ) ， 同 时 ， 在 一 
定 程 度 上 去 除 前 面 工序 加 工时 所 形成 的 残留 凹凸 层 和 裂纹 层 ， 并 修复 加 工时 产 
生 的 型 面 误差 , 保 证 加 工 工件 满足 形状 、 位 置 精度 、 表 面 粗 糙 度 、 尺 寸 等 方面 
要 求 。 加 工 过 程 的 关键 问题 之 一 是 对 研磨 工具 位 置 姿 态 的 控制 和 人 研磨 作用 力 的 
控制 。 控 制 研 磨 工 具 在 给 定 的 路 径 轨迹 和 速度 下 ， 保 证 适当 的 磨 削 头 与 接触 表 
面 之 间作 用 力 ， 以 达到 去 除 半 精 加 工 残 留 余 量 的 目的 。 

研磨 的 方法 不 同 ， 影 响 的 因素 如 图 6- 1 PRU, 
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研磨 相当 于 磨 削 头 上 磨料 颗粒 对 待 磨 削 表面 的 微小 切削 作用 ， 参 考 文献 
[159] 认为 抛光 与 研磨 的 去 除 机 理 是 相同 的 ， 都 是 坚硬 的 磨料 颗粒 对 待 加 工 
表面 进行 机 械 作用 。 目 前 学 术 界 还 没有 形成 一 个 能 表达 研磨 机 理 的 统一 理论 模 
型 ， 普 遍 认 可 的 说 法 是 : 工具 头 的 旋转 速度 、 工 具 头 的 材料 、 人 研磨 压力 等 因素 
是 决定 研磨 效率 及 研磨 质量 的 关键 1%, 197 

Preston ЖЖ 170, 1 是 最 著名 的 研磨 材料 去 除 模型 之 一 ， 该 模型 通过 对 加 
工 过 程 中 的 曲面 曲率 进行 控制 ， 实 现 对 材料 去 除 率 的 预测 ， 从 而 提高 工件 被 研 














磨 表面 的 质量 。 
Preston 模型 为 
= Ky xp xo (6-1) 
式 (6-1) 中 : dh/dt 一 一 材料 的 去 除 率 ; 
Kp Preston 系数 ; 





一 一 研磨 面 上 的 压力 ; 
磨 前 头 与 工件 表面 之 间 的 相对 线 速度 。 

由 于 受到 竺 加工 表面 的 材料 性 能 和 特性 限制 ， 在 实际 的 金属 表面 研磨 加 工 
中 , p 和 w 将 在 合适 的 参数 值 范围 内 满足 式 (6-1); p 和 w 与 研磨 时 工具 头 的 
姿态 相关 。 确 定 合理 的 研磨 姿态 与 研磨 路 径 ， 可 以 提高 研磨 效率 和 型 面 质量 。 





p 








6.2 实验 平台 


采用 本 文 开发 的 5 — TTRRT 人 研磨 机 器 人 系统 进行 自由 曲面 自主 研磨 实验 ， 
实验 系统 如 图 6-2 所 示 ， 主 要 由 以 下 部 分 组 成 : 

1) 5 - TTRRT 研磨 机 器 人 : 规格 550mm x550mm x335mm; 重 约 30kg。 

2) 自制 被 动 柔性 研磨 工具 头 ， 规 格 625mm, HE 240#。 

3) 控制 计算 机 : 处 理 器 主 频 为 2. 53GHz， 内 存 为 2GB。 

4) 六 轴 运 动 控制 卡 ， 型 号 为 ADT - 856。 

5) SRM -1 (D) 型 表面 粗糙 度 测 量 仪 ， 量 程 0. 025 ~ 6. 30um。 

研磨 机 器 人 系统 实物 图 见 图 6-2, 
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图 6-2 研磨 机 右 人 系统 实物 图 


6.3 机 器 人 研磨 实验 


针对 式 (3-7) 给 定 的 研磨 工具 头 的 期 望 轨迹 ， 选 用 球 头 刀具 ， 采 用 行 切 
法 进行 加 工 ， 应 用 上 一 章 的 模糊 阻抗 控制 方法 ， 期 望 的 研磨 作用 力 定义 在 工件 
坐标 系 的 Z 方 向， 大 小 为 15N。 在 机 器 人 人 研磨 控制 系统 中 ， 通 过 对 各 关 方 的 脉 
冲 控制 量 取得 对 应 的 位 移 ， 经 过 运动 学 正解 得 到 研磨 工具 头 的 实际 位 移 ， 由 安 
装 在 研磨 工具 末端 的 力 传感器 测量 加 工 过 程 中 作用 于 研磨 工具 的 研磨 力 ， 实 验 
采集 的 位 移 和 研磨 力 在 工件 坐标 系 中 的 表示 ， 分 别 如 图 6-3 和 图 6-4 所 示 。 





а. „А ни Е Á. 2. — de aA. 
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时 间 /s 





6-3 ”位 移 轨迹 
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时 间 /s 








图 6-4 采集 的 研磨 作用 力 


由 图 6-3 可 知 ， 在 平行 于 yi01z1 的 平面 内 ， 在 位 置 控制 的 z 方 向 上 ， 位 置 
误差 小 于 0. 5mm。 期 望 研 磨 力 为 15N， 在 z 的 正方 向 上 ,测量 的 作用 力 与 期 望 
力 的 偏差 为 +2N。 


6.4 ”研磨 工艺 参数 对 研磨 效果 影响 的 正 交 试验 


6.4.1 因素 水 平 确定 


在 机 器 人 研磨 试验 中 ， 通 过 对 研磨 工具 头 姿态 控制 原理 分 析 ， 可 以 发 现 研 
磨 工 具 头 与 工件 间 的 法 向 作用 力 F. Лх ATT3EXEJE v (机 器 人 研磨 工具 头 治 
工件 表面 的 运行 速度 ) 、 工 具 头 主轴 转速 n 和 工具 头 姿态 角 B， 这 四 种 因素 对 
工件 表面 的 加 工 质量 有 很 大 影响 。 

本 文采 用 正 交 试验 法 对 4 因素 4 水 平 上 6 (4?) 进行 正 交 试验 ， 全 面 分 析 
研磨 时 ， 上 述 四 个 因素 对 表面 加 工 质量 的 影响 ， 建 立 的 研磨 正 交 设计 因素 水 平 
表 如 表 6-1 所 示 ， 研 磨 正 交 实验 结果 如 表 6-2 所 示 。 
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表 6-1 因素 水 平 表 










































































































































































Аж 法 向 作用 力 进 给 速度 工具 头 转速 姿态 

水 平 F/N »/ ( mm/min) n/ ( r/ min) B/(°) 
1 水 平 5 30 1000 18 
2 水 平 10 60 1500 25 
3 水 平 15 120 2000 28 
4 水平 20 180 2500 35 

表 6-2 研磨 正 交 实验 结果 
d£ 法 向 作用 力 LE ftit 进 给 速度 姿态 角 粗糙 度 
F/N »/ ( mm/min) n/ ( y/ min) B/(°) Ra/ pm 

试验 1 1 1 1 1 0. 962 
试验 2 1 2 2 2 0. 861 
试验 3 1 3 3 3 0. 933 
试验 4 1 4 4 4 1. 101 
试验 5 2 1 2 3 0. 931 
试验 6 2 2 1 4 0. 835 
试验 7 2 3 4 1 0. 902 
试验 8 2 4 3 2 0. 983 
试验 9 3 1 3 4 0. 901 
试验 10 3 2 4 3 0. 746 
试验 11 3 3 1 2 0. 862 
试验 12 3 4 2 1 0. 943 
试验 13 4 1 4 2 0. 922 
试验 14 4 2 3 1 0. 810 
试验 15 4 3 2 4 0. 893 
试验 16 4 4 1 3 1. 026 
均值 1 0. 96425 0. 929 0. 92125 0. 92125 
均值 2 0. 91275 0. 813 0. 907 0. 907 
均值 3 0. 863 0. 8975 0. 90675 0. 909 
均值 4 0. 91275 1. 01325 0. 91775 0. 9325 

极 差 0. 10125 0. 20025 0. 0145 0. 0255 
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6.4.2 各 主要 因素 对 研磨 效果 的 影响 


在 4 个 主要 因素 中 ( 见 表 6-1) ， 保 持 其 中 3 个 因素 的 工艺 参数 不 变 ， 调 
整 其 中 一 个 主要 影响 因素 ， 可 以 研究 各 主要 因素 对 研磨 效果 的 影响 。 

HL. (45) 正 交 试验 表 ， 进 行 多 组 正 交 试验 ,结合 表 6-2 所 得 的 正 交 
试验 结果 ， 可 以 绘制 法 向 作用 力 、 进 给 速度 v、 工 具 头 转速 п 及 工具 头 姿态 
Ж B 等 因素 对 表面 粗 烟 度 的 影响 曲线 ， 如 图 6-5 所 示 。 








ЭДЕ 








0 500 1000 1500 2000 2500 3000 
T-RGESGHlU(r/min) 
a) TR dos An RB ЛЕН Son 


Ir 一 一 
0.8 г 


zy FRU BEAL /um 








0 5 10 15 20 25 
法 向 作用 力作 


b) 法 向 作用 力 F 对 表面 粗 烽 度 的 影响 
图 6-5 各 因素 对 表面 粗糙 度 的 影响 曲线 
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d) EAS fü 8 对 表面 粗糙 度 的 影响 




















图 6-5 各 因素 对 表面 粗糙 度 的 影响 曲线 ( 续 ) 


6.4.3 实验 结果 分 析 


分 析 表 6-2 中 每 个 单 因 素 在 不 同 水 平时 对 表面 粗糙 度 的 影响 规律 ， 可 以 得 
到 在 不 考虑 交互 作用 的 情况 下 ， 最 优 水 平 加 工 条 件 是 正 交 化 实验 各 因素 之 间 的 
好 水 平 组 合 ， 由 此 得 到 的 本 次 实验 最 优 组 合 为 Favon4B,, ЖР: 法 向 作用 力 F 
为 10N， 进 给 速度 " J 6mm/min, 工具 头 转速 为 2000r/min， TAS VES ffi В 
为 25° 时 ， 研 磨 效 果 最 佳 。 

结合 图 6-5， 各 因素 对 表面 粗糙 度 的 影响 分 析 如 下 : 
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1) МИ 6-5a 中 可 以 得 出 : 工件 表面 粗糙 度 值 随 着 工具 头 转速 逐渐 增 大 体 
现 出 先 下 降 然后 再 上 升 的 变化 过 程 ， 在 工具 头 转速 达到 临界 值 前 ， 粗 糙 度 值 单 
调 下 降 ， 达 到 或 超过 临界 值 后 开始 单调 上 升 。 由 Preston 方程 可 知 ， 随 着 接触 
点 相对 线 速度 增 大 ， 研 磨 材料 去 除 率 也 成 比例 地 增 大 ， 被 加 工 工件 表面 的 粗 烽 
度 最 优 出 现在 材料 去 除 率 与 表面 粗糙 度 达 到 一 临界 最 佳 平衡 点 位 置 。 此 后 ， 研 
磨 区 内 磨 削 头 与 加 工 的 表面 之 间 的 相对 线 速 度 增 大 ， 表 现在 它们 之 间 的 接触 表 
面 上 的 剪 切 力也 增 大 ， 虽 然 随 着 剪 切 力 的 增 大 ， 磨 削 力 更 强 ， 对 工件 表面 的 去 
除 作用 也 更 大 ,但 是 超过 这 个 临界 点 ， 将 导致 加 工 表面 的 粗糙 度 值 变 大 。 上 面 
分 析 了 磨 前 尖 的 转速 对 加 工 表 面 粗糙 度 的 有 影响， 在 同样 的 磨 前 头 材 料 、 结 构 和 
加 工 表 面 情 况 下 ， 保 持 磨 削 头 在 临界 相对 线 速度 的 附近 时 ， 可 得 到 最 好 的 加 工 
表面 。 

2) 从 图 6-5b 中 得 出 : 工件 表面 粗糙 度 值 随 着 法 向 作用 力 正 增 大 而 减 小 ， 
当 继续 增 大 ， 超 过 它 的 临界 值 时 ， 粗 糙 度 值 反而 增 大 。 虽 然 增 大 法 向 作用 力 下 
能 够 清除 工件 表面 的 凸 峰 ， 可 以 使 加 工 表面 的 粗糙 度 值 减 小 。 随 着 法 向 作用 力 
下 的 增 大 ， 工 件 表 面 与 磨 肖 头 之 间 的 接触 面积 变 大 ， 导 致 磨 削 头 作用 在 工件 加 
工 表面 上 的 剪 切 力 和 正 压 力 增 大 ， 导 致 接触 表面 上 研磨 颗粒 对 金属 的 加 工 表 面 
的 刊 除 作用 力也 同时 增 大 ， 工 件 表面 的 研磨 六 迹 更 为 明显 ， 研 磨 效果 将 变 差 。 

3) 从 图 6-5c 可 得 出 ， 工 具 头 进 给 速度 对 研磨 效果 的 影响 较 小 ， 说 明基 于 
位 置 阻抗 模型 的 协调 控制 可 以 满足 工具 头 顺 应 被 加 工 曲 面 的 要 求 ， 所 以 取得 较 
好 的 效果 。 

4) 图 6-5d 给 出 了 工具 头 姿态 角 对 研磨 效果 的 影响 ， 在 姿态 角 小 于 20° 
时 ， 工 具 头 的 头 部 区 域 工作 ， 接 触 区 域 半 径 小 导致 研磨 位 置 线 速度 低 ， 所 以 工 
件 表 面 磨 削 的 质量 较 差 。 当 工具 头 的 姿态 角 处 于 20° ~30" 之 间 时 ， 加 工 工件 
的 表面 粗糙 度 值 没 有 明显 的 变化 ， 这 时 说 明 工 具 头 姿态 角 的 变化 对 表面 的 加 工 
质量 影响 小 。 当 姿态 角 超过 30" 时 ， 工 具 头 与 工件 表面 的 接触 区 域 为 圆柱 面 ， 
适应 被 加 工 曲面 曲率 的 能 力 降 低 ， 所 以 工件 表面 磨 削 的 质量 较 差 。 因 此 ， 从 机 
器 人 系统 的 结构 、 工 具 头 的 尺寸 并 结合 实验 数据 分 析 ， 选 择 25" 的 抛光 工具 头 
姿态 角 的 控制 参数 。 
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v-2Juqi + Jp 43 з +Jin Gn 
w =Jaqi + Jada T + Jud, 
对 于 转动 关节 i 有 
(рхп), п, 
Tus (рхо), |, a=|o; 
(pxa), а, 
对 于 移动 关节 有 
n; 0 
Tu = |о, |, Ja =| 0 
a, 0 
各 连 杆 至 末端 连 杆 的 变换 为 
—$4 -cs4 C3C4 Cac d, +47s3 —110с; -272 
Ip, = 0 C4 54 5445 +d, 
—€4 5354 = 8364  —s3C4ds +47c3 +1105; +d, 
0 0 0 1 
—$4 = 6384 C3C4 636405 +475, —110c, – 110c} -222 
2р, = C4 = 5354 $4C4 sac d, +47c, — 1105; 
0 C4 54 s4ds 
0 0 1 
0 —s c4 cds -110 
TR 1 0 0 -47 
i Оа s4 s4ds 
0 1 
0 0 -1 -d 
0 1 0 
0 -47 
0 0 1 
各 个 关节 的 雅克 比 列 向 量 
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2d 0 cad, — 110 0 0 
5354 Сд = 4754 = d; 0 
M = т `h = А Ја Ыс ‚А = y ‚Л; 
0 0 y І ° 
0 0 C4 0 0 
0 0 5 0 : 
TR ERE TE ЕЗЕР: J (q) 
-c 0 cd; -110 0 0 
$454 C4 -4Ts, -ds 0 
= $4€4 $4 47c4 0 1 
J(q) = 
0 0 0 1 0 
0 0 Ж 0 0 
0 0 54 0 0 
Ј" х =ЈЈ д 
а= (QI) 8 
4. 速度 分 析 
I i- d i i N 
T, - TT Ti (Tii) Ti* Ty 
di 
000 0 
000 0 
d mox d 
ge et ES dis ; 
d, | da, i 0 0 4; 
0 0 0 0 
x iE 
y X5 
_ |=Ф\ТФ,Т\Ф,Т;Ф,Т;Ф,Т 
25 
1 1 
1000 1 0 0 -50 
0 1 0 0 0 0 1 d 
Т! = Ti = : 
0.0 1 d, 0 -1 0 0 
0.00 1 0 0 0 1 
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c =s, 0 0 100 -22 
Ф - s G 0 0 т = 0 1 0 0 
0 0 1 0 0 0 1 0 
0 0 0 1 0 0 0 1 
а —s Ü 0 10 O - 110 
ds $4 c, 0 0 ті = 0 0 -1 0 
0 0 1 0 0 1 0 0 
0 0 0 1 0.0 0 1 
10 0 - 110 0 0 1 d; 
0 0 -1 0 1 
Ti = 7 = 0 0 
0 1 0 0 -1 0 0 47 
0 0 0 1 0 00 1 
А P X5 
i diy ys 
y sN y - (W, 0,0, Top Tr. Тф, T3 5 T; + + Wh Туф, Ti 03150, T35T; ) 
Р 1 25 
2 
l l 
0 
0 -1 0 0 
1 0 00 
6-570000 
0.0 00 
0 0 0 
7 0 0 : 0 
WT. = А To = , 
000 d, 000 d, 
000 0 000 0 
-c4 Ü -s4 (110 -d; )sz34 +47с * 2225, 
. 0 0 0 0 
T53 = 
зм 0 -ca (110 -d;)c34 – 47534 +220c3 
0 0 0 0 
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附 录 
-c34 Ü -s4 (10-405 ) 534 -47c34 
Ты _ 0 0 0 0 
s34 0 -ca (110-d)c4 - 4753, 
0 0 0 0 
000 0 
А 000 0 
Ts, = 
0 0 0 d; 
000 0 
2 0 0 0 х; 
ae 0 0 0 ^L у» А 
2 000 d 25 
0 000 0 1 
—c Ü -s4 (110 -d;)sz34 +47с +2225; х5 
m 0 0 0 0 К ys 
$4. Ü -ca (110-d3)e44 - 4734 +222c3 2 
0 0 0 0 1 
-c4 Ü -s4 (10-45) 534 -47c34 "s 
+04 a i x ^s + 
54 0 -ca (10-а ) езд -47c34 25 
0 0 0 0 1 
0 0 х5 0 х; 
кн 75 i J5 
x + = |x 





000 0/ UJ моо 07 M 
5. 加 速度 分 析 


X x Xs 

y d ` .. " Е .. z ys 

w^ dr 7 = (Т +75 + Т + Т5 + Ту) 

2 . Z5 
2 

0 А 1 
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0 0 0 
. |000 
Ts, в 
0 0 0 d, 
000 0 
000 0 
. |0 00 0 
Ts) = 
0 0 0 d, 
000 0 


Ts; = 03, 150, 7,610. Tida Tios T; + 03040, 1505 Тү Ө; ф» 150,9, 50; T; +03, Туф» Tt 
00, Т» Фф, 15; T 


Ts 2030940 Tho, 116,0, Toad, Tos r +040, Tho, Тф, 1560, Т5Ф; T * 040, Tho, n 








93313 b4T3bs T; 
000 0 
- 000 0 
Tss = Ё 
0004 
000 0 
53403 (03 +04) -03C34 (348403 (05 +04) -0353454 -0340403 (W3 04) – 0393464 
F, _ 0 0 0 
= 63403 (#3 +04) 05534 75545405 (03 +@4) +03C3454 — 5340403 (W3 +04) - 056344 
0 0 0 
( —сзд®з(с4@; - 110) -47534) (3 +04) +222c,03 +0 ( —5з4 (ca ds -110) +41с +2225) 
0 
( -s3403 (c4ds -110) -47c34 ) (w3 +04) -2225303 + a3( -c34 (ea ds -110) -47334 +222c3 ) 
0 
53404 (03 +04) 05034 348404 (03 +@4) -0453454 – 0345404 (W3 +4) — 04534€4 
8 0 0 0 
Ts, = 


= с3404 (03 +04) t04534 -8345404 (93 +04) +A4C3454 5340404 (93 +04) 0403404 


0 0 0 
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( -c34w4(c4ds5 - 110) 247544) (оз +04) * o4 -534 (C4ds -110) +47c34 +22253) 
0 

( 753404 (ca ds -110) -47e44) (оз +04) +04 ( -c34 (c4 ds - 110) -47554 +2220) 

0 


6. 动力 学 分 析 
机 絮 人 动力 学 方程 最 终 表达 式 如 下 为 


= 2, 0,4 + Ly qi + 之 È Py d: q, + D. 
J= izljz 


n 


D,= Y m, | [^0,&2. "8, «8, k2 "8, +81298] + [P d, * p] 
p = тахі,ј 


+ [?r, + (d; x8; +74, х?б,) ] | 


п 
_%=1 == 
D, m gdm, Tp 








p= 
R1 各 连 杆 系 的 质量 
连 杆 序号 i m, m, тз тд ms 
质量 /kg 21.6 19.0 12.4 9.1 3.3 
R2 各 连 杆 系 的 转动 惯量 
转动 惯量 /kg + n? Б Ly L, Lom Ly Lm 





0. 145 0. 074 0. 193 0. 066 0. 093 0. 081 














TE, 由 于 第 1、2、5 连 杆 系 均 为 平 动 关节 ， 其 转动 惯量 均 没 有 意义 。 
RI 各 连 杆 系 的 质心 矢量 














з = 3 4. 5 
ry T5 rs T4 Ts 


! 
N 





0. 175i -0.007j -0. 09i - 0. 0085 


质心 矢量 —0. 137i -0. 137i -0. 028i – 0. 013k 
+0. 012k —0. 018k 


HE, 表达 式 中 的 i j、 分 别 表示 参考 x*、y 和 z 轴 的 各 矢量 ， 缺 项 的 即 为 零 。 
对 于 转动 关节 


Р cu el 1-1 i-l 1-1 

dix = Nox Poy + Пру Ррх 
Р ee | i-l i-l 1-1 

diy ~ Opx Ppy + O py Pox 
p Lud] i-l i=1 i=1 

d; m apx Ppy * ару Ррх 


| 
6; = Hab aug а yk 


对 于 平 动 关节 , 平移 矢量 为 "d， = 十 "p.a apk 
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旋转 矢量 为 "5; =0 + 0; + ОК 
各 个 关节 的 平移 矢量 和 旋转 矢量 为 


'd, 20i +0j +k 16, 20i 0j - Ok 
?d, =0i - 0j - ОЕ 26, = 0i - 1j + Ok 
3d, = -szi — суу +0Ё 36, 20i + 0j +0k 
^d, = -= 53041 +5354] + csk ^8, 20i + 0 «Ok 
^d, = —csi + 35а) — s3cak ?8, = 0i +07 + 0k 
24, -0i -j + Ok 26, = 0i +07 + Ok 
ЗЯ, = —ssi-— cj +0Ё 36, = 0i +07 + Ok 
^d, = —s3cai + 5454] + cak 48, 20i + 0j «Ok 
^d, = —сзї +5354] — 5сдЁ ?6, 20i € 0j «Ok 
3d, = -2225,i -222e4j +0Ё 384, = 01 +07 + 1k 
^d,  -222s,c,i — 2225354] + 11052 +11002 +222c 48; =s,i + сај +0Ё 
564, - 0i + сај +s4k 
5d = (са; – 2220, — 110) + ( — 475, + 2225454) j + (4704 = 222546, = 
110c, зс) 
^d, =0i «0j + 110k 46, = +s4i + сај +0Ё 
`d, = (сй; - 110) i -47s4j +47, ó, =Oi e cj + sk 
^d; = -i +0j «OK 56; = 0i +07 +0k 


对 于 平 动 关节 ,?6; =0,?d; + "d; 21, WD, = У, m, 


pai 


5 5 
Dy, = Ўт, = 65.4 Dy = Dim, = 43.8 Ds; = m; = 3.3 


р=1 р=2 


D =0. 14552 +0. 074c — 1. 070c; +3. 4412 


Dis =0. 063753 — 1. 5925c, +0. 0112c, +0. 01s4 +3. 0576 
D =0 

05 =0 

D = —1. 2925s, +0.0637c; 43.0576 

Dy, =0 

D, =0 


Dy 


=0. 159s3c, +0. 0595c2c, +0. 174c554c4 +0. бсзсд — 1. 928с +5. 7711 


附 Ж 91 





D44 = —0. 066s,5,c4 +0. 093332 – 0. 11225354 

D3; = -0. 11235354 

Dy, =0. 027c2 -0. 02306c, — 0. 0198s, +3. 4974 

Dj; = —0. 1551s, +0. 3399 

对 于 转动 关节 ,'-ig= (-g-o gen 0 0) 

对 于 平 动 关节 ,8=(0 0 0 -g-a), n、0o、4a 均 为 “7, 的 列 向 量 。 
°g=(0 0 0 0) 

lg=(0 0 0 0) 

?g-(-85 ge 0 0) 

38=(-8s4 g4 0 0) 


i1=(-0.317 0 0) 

17,=( -0.317 0 0) 

07, = (0.175c3 +0.012s, -0.175s3 +0.012c —0.007) 

17, = (0. 175c, +0. 0075 0. 1755 -0.007c 0.012) 

27, = (0. 175c, +0. 0075 -0.012 0. 1755; –0. 007c; ) 

Or =( -0.09c,c, –0. 0085, 0. 09c3s4 +0. 008; -0.018c;) 

Ir, = ( —0.09сус +0. 018s3c4 0. 09c3s4 -0.018s,s, 0.018, —0. 018c; ) 

27, =( -0.09сзс +0. 0185 0. 095, –0.018с -0.09sc –0. 018c3) 
F4 = ( -0.09c, +0. 0085, -0.018 0. 09, +0. 008с,) 

о, = (0. 02853 +0.013c 0. 028c3s4 –0.013535, — — 0. 028c3c4 +0. 0135354) 
F, = (0. 0285, +0. 013, 0. 028c3s4 –0. 013535, —0.028сус +0. 0135354) 
27, = (0. 028суз +0.013c3ca 0. 028с, +0. 013s, 0. 028538, +0. 0135,c,) 
37, = (0.0285, +0. 013c, 0 0. 028, -0. 01354) 


^r; = (0.013 0 0.028) 
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重力 项 D; 为 

D, =D, =D, =0 

D, = -0. 38192 -0. 1916c, -1. 1116s, 

D, = (0.0429 + 1.1116c, + 0.09925, ) s3g + ( — 0.3156 - 0.0924c, + 
0. 0429s, ) cag 

将 关节 耦合 惯量 项 D; 和 重力 项 Р, 代入 ， 即 可 以 求解 出 各 个 关节 处 的 驱动 
FMA TVET; 











к=з 
oo 
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